devbox
Monodyee
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

双目相机三维坐标计算与视差计算，python-opencv实现_cv2.ximgproc.createrightmatcher

作者：Monodyee | 2024-02-27 12:08:09

赞

踩

cv2.ximgproc.createrightmatcher

原理我就不说了，就是各种公式推导和坐标转换再加上一些计算方法。

要使用这个代码首先你得有自己采集的双目图片，或者可以用cv自带的来运行代码。

其次该代码也是自己综合了许多人的代码略微调整修改了，可以说是缝合怪。其中如何匹配左右图像的点（貌似可以极线校正后利用极线匹配来做，但我好像不知道怎样做）我是自己想的，可能很粗糙，也不知道对不对。思路就是：通过给左右图片的内角点标上序号，然后左右相机的相同序号的内角点作为一组点位代入三维坐标求解函数。

直接上代码吧！我自己做的实验不理想（拍摄的图片找不到角点，图片分辨率太大不知道缩小分辨率是否会对标定有影响），但我不知道是代码问题还是啥，就发出来让大家一起学习与指导一下。

第一个版本：matlab标定，opencv处理

  1. import numpy as np
  2. import cv2
  3. import os
  4. import  xml.dom.minidom
  5.  
  6. # 遍历图片
  7. def getFileList(dir,Filelist, ext=None):
  8.     """
  9.     获取文件夹及其子文件夹中文件列表
  10.     输入 dir：文件夹根目录
  11.     输入 ext: 扩展名
  12.     返回： 文件路径列表
  13.     """
  14.     newDir = dir
  15.     if os.path.isfile(dir):
  16.         if ext is None:
  17.             Filelist.append(dir)
  18.         else:
  19.             if ext in dir[-3:]:
  20.                 Filelist.append(dir)
  21.     
  22.     elif os.path.isdir(dir):
  23.         for s in os.listdir(dir):
  24.             newDir=os.path.join(dir,s)
  25.             getFileList(newDir, Filelist, ext)
  26.  
  27.     return Filelist
  28. # 计算内焦点世界坐标
  29. def calRealPoint(row,col,trueLength):
  30.     imgpoint = []
  31.     for rowIndex in range(0,col):
  32.         for colIndex in range(0,row):
  33.             imgpoint.append([colIndex*trueLength,rowIndex*trueLength,0])
  34.     imgpoint = np.array(imgpoint,np.float32)
  35.     return imgpoint
  36. # 画线
  37. def draw_line1(image1, image2):
  38.     # 建立输出图像
  39.     height = max(image1.shape[0], image2.shape[0])
  40.     width = image1.shape[1] + image2.shape[1]
  41.     
  42.     output = np.zeros((height, width,3), dtype=np.uint8)
  43.     output[0:image1.shape[0], 0:image1.shape[1]] = image1
  44.     output[0:image2.shape[0], image1.shape[1]:] = image2
  45.  
  46.     for k in range(15):
  47.         cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA)  # 直线间隔：100
  48.  
  49.     return output
  50. # 三维坐标计算
  51. def uvToXYZ(lx, ly, rx, ry):
  52.     mLeft = np.hstack([leftRotation, leftTranslation])
  53.     mLeftM = np.dot(MLS, mLeft)
  54.     mRight = np.hstack([R, T])
  55.     mRightM = np.dot(MRS, mRight)
  56.     # print(mLeft)
  57.     # print(mLeftM)
  58.     A = np.zeros(shape=(4, 3))
  59.     for i in range(0, 3):
  60.         A[0][i] = lx * mLeftM[2, i] - mLeftM[0][i]
  61.     for i in range(0, 3):
  62.         A[1][i] = ly * mLeftM[2][i] - mLeftM[1][i]
  63.     for i in range(0, 3):
  64.         A[2][i] = rx * mRightM[2][i] - mRightM[0][i]
  65.     for i in range(0, 3):
  66.         A[3][i] = ry * mRightM[2][i] - mRightM[1][i]
  67.     B = np.zeros(shape=(4, 1))
  68.     B[0][0] = mLeftM[0][3] - lx * mLeftM[2][3]
  69.     B[1][0] = mLeftM[1][3] - ly * mLeftM[2][3]
  70.     B[2][0] = mRightM[0][3] - rx * mRightM[2][3]
  71.     B[3][0] = mRightM[1][3] - ry * mRightM[2][3]
  72.     XYZ = np.zeros(shape=(3, 1))
  73.     # 采用最小二乘法求其空间坐标
  74.     cv2.solve(A, B, XYZ, cv2.DECOMP_SVD)
  75.     # print('坐标为：\n',XYZ)
  76.     return XYZ
  77. # 获取像素坐标
  78. def get_pixel(img,index):
  79.     gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
  80.  
  81.     _,corners = cv2.findChessboardCorners(gray,(row,col))
  82.     criteria = (cv2.TermCriteria_EPS+cv2.TermCriteria_MAX_ITER,30,0.01)
  83.     corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
  84.     return corners2[index].ravel()
  85. # 从xml(matlab生成)读取内外参数等
  86. def getParams(file):
  87.     dom = xml.dom.minidom.parse(file)
  88.     root = dom.documentElement
  89.     datas = root.getElementsByTagName('data')
  90.     after_datas = []
  91.     for data in datas:
  92.         data = data.firstChild.data
  93.         data = data.replace('\n', '').replace('\r', '')
  94.         data = data.split(' ')[0:-1]
  95.         data = [float(x) for x in data]
  96.         after_datas.append(data)
  97.     MLS = np.array(after_datas[0]).reshape(3,3)
  98.     MRS = np.array(after_datas[1]).reshape(3,3)
  99.     dLS = np.array(after_datas[2])
  100.     dRS = np.array(after_datas[3])
  101.     R = np.array(after_datas[4]).reshape(3,3)
  102.     T = np.array(after_datas[5]).reshape(3,1) 
  103.     return MLS,MRS,dLS,dRS,R,T 
  104. # 清空文件
  105. def del_file(path):
  106.     ls = os.listdir(path)
  107.     for i in ls:
  108.         c_path = os.path.join(path, i)
  109.         if os.path.isdir(c_path):
  110.             del_file(c_path)
  111.         else:
  112.             os.remove(c_path)
  113.  
  114. del_file('./left_')
  115. del_file('./right_')
  116. del_file('./left_re')
  117. del_file('./right_re')
  118. leftRotation = np.array([[1, 0, 0],
  119.                          [0, 1, 0],
  120.                          [0, 0, 1]])
  121. leftTranslation = np.array([[0],
  122.                             [0],
  123.                             [0]])
  124. # 棋盘格内焦点个数横纵
  125. # 参数读取
  126. MLS,MRS,dLS,dRS,R,T = getParams('2th.xml')
  127. print('L内参\n',MLS)
  128. print('L畸变\n',dLS)
  129. print('R内参\n',MRS)
  130. print('R畸变\n',dRS)
  131. print('旋转\n',R)
  132. print('平移\n',T)
  133. row = 9
  134. col = 6
  135. # 格子真实尺寸
  136. trueLength = 10
  137. # 图片后缀
  138. hz = 'jpg'
  139. # 遍历图片
  140. imglistL = getFileList('./left',[],hz)
  141. imglistR = getFileList('./right',[],hz)
  142. # 创建存储能被角点检测到的图片的文件夹
  143. folderL = './left_'
  144. folderR = './right_'
  145. if not os.path.exists(folderL):
  146.     os.mkdir(folderL)
  147. if not os.path.exists(folderR):
  148.     os.mkdir(folderR)
  149. # 首先检测图片角点检测是否正确
  150. for i in range(0,len(imglistL)):
  151.     ChessImaR = cv2.imread(imglistR[i],0)
  152.     ChessImaL = cv2.imread(imglistL[i],0)
  153.     # 角点检测
  154.     retR, cornersR = cv2.findChessboardCorners(ChessImaR,
  155.                                                (row,col),None)
  156.     retL, cornersL = cv2.findChessboardCorners(ChessImaL,
  157.                                                (row,col),None)
  158.     # 只要一对图片中有一张不能检测角点，这对图片便舍弃
  159.     if (retR == True) & (retL == True):
  160.         print('第%d对图片符合'%i)
  161.         # 同时存入另一个文件夹
  162.         cv2.imwrite(folderL+'/img'+str(i)+'.'+hz,ChessImaL)
  163.         cv2.imwrite(folderR+'/img'+str(i)+'.'+hz,ChessImaR)
  164.         # 画出检测的角点
  165.         cv2.drawChessboardCorners(ChessImaL, (row, col), cornersL, retL)
  166.         cv2.drawChessboardCorners(ChessImaR, (row, col), cornersR, retR)
  167.         cv2.imshow('imgL',ChessImaL)
  168.         cv2.imshow('imgR',ChessImaR)
  169.         if cv2.waitKey(0) & 0xFF == ord(' '):
  170.             continue
  171.         if cv2.waitKey(0) & 0xFF == 27:
  172.             break
  173.     else:
  174.         print('第%d对图片不符合,剔除'%i)
  175. # 遍历筛选后图片
  176. imglistL_ = getFileList(folderL,[],hz)
  177. imglistR_ = getFileList(folderR,[],hz)
  178. # 参数含义不知
  179. rectify_scale = 0 
  180. # Q重投影矩阵
  181. RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(MLS, dLS, MRS, dRS,
  182.                                                  ChessImaR.shape[::-1], R, T,
  183.                                                  rectify_scale,(0,0),alpha=-1)
  184. # 畸变校正
  185. Left_Stereo_Map = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
  186.                                              ChessImaR.shape[::-1], cv2.CV_16SC2)
  187. Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
  188.                                               ChessImaR.shape[::-1], cv2.CV_16SC2)
  189. # 参数设置参考官方例程
  190. window_size = 3
  191. min_disp = 16
  192. num_disp = 112-min_disp
  193. stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
  194.     numDisparities = num_disp,
  195.     blockSize = 16,
  196.     P1 = 8*3*window_size**2,
  197.     P2 = 32*3*window_size**2,
  198.     disp12MaxDiff = 1,
  199.     uniquenessRatio = 10,
  200.     speckleWindowSize = 100,
  201.     speckleRange = 32
  202. )
  203. # 使用滤波器降低噪声
  204. stereoR=cv2.ximgproc.createRightMatcher(stereo)
  205. # 过滤器参数设置
  206. lmbda = 80000
  207. sigma = 1.8
  208. visual_multiplier = 1.0 
  209. wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=stereo)
  210. wls_filter.setLambda(lmbda)
  211. wls_filter.setSigmaColor(sigma)
  212. # 创建保存极线校正的图片
  213. folderL_ = './left_re'
  214. folderR_ = './right_re'
  215. if not os.path.exists(folderL_):
  216.     os.mkdir(folderL_)
  217. if not os.path.exists(folderR_):
  218.     os.mkdir(folderR_)
  219. # 标定参数使用
  220. for i in range(0,len(imglistL_)):
  221.     frameR = cv2.imread(imglistR_[i])
  222.     frameL = cv2.imread(imglistL_[i])
  223.     # Rectify图片，极线校正
  224.     Left_nice = cv2.remap(frameL,Left_Stereo_Map[0],Left_Stereo_Map[1], cv2.INTER_AREA)
  225.     Right_nice = cv2.remap(frameR,Right_Stereo_Map[0],Right_Stereo_Map[1], cv2.INTER_AREA)
  226.     # 保存极线校正后的图片
  227.     cv2.imwrite(folderL_+'/img'+str(i)+'.'+hz,Left_nice)
  228.     cv2.imwrite(folderR_+'/img'+str(i)+'.'+hz,Right_nice)
  229.     # 显示极线校正的图片
  230.     res1 = draw_line1(Left_nice,Right_nice)
  231.     cv2.imshow('ln',Left_nice)
  232.     cv2.imshow('rn',Right_nice)
  233.     cv2.imshow('res1',res1)
  234.     # 计算距离三维坐标
  235.     u11,v11 = get_pixel(Left_nice,0)
  236.     u21,v21 = get_pixel(Right_nice,0)
  237.     u12,v12 = get_pixel(Left_nice,1)
  238.     u22,v22 = get_pixel(Right_nice,1)
  239.     p1 = uvToXYZ(u11,v11,u21,v21)
  240.     p2 = uvToXYZ(u12,v12,u22,v22)
  241.     print('p1坐标\n',p1)
  242.     print('p2坐标\n',p2)
  243.     print('距离\n',np.linalg.norm(p1-p2))
  244.     ################################################
  245.     grayR= cv2.cvtColor(Right_nice,cv2.COLOR_BGR2GRAY)
  246.     grayL= cv2.cvtColor(Left_nice,cv2.COLOR_BGR2GRAY)
  247.     # 计算深度，参考官方例程
  248.     dispL= stereo.compute(grayL,grayR).astype(np.float32)/ 16.0 
  249.     dispR= stereo.compute(grayR,grayL).astype(np.float32)/ 16.0
  250.     # 使用滤波器降噪,filteredimg为过滤后的深度图
  251.     filteredImg= wls_filter.filter(dispL,grayL,None,dispR)
  252.     filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, 
  253.                                 norm_type=cv2.NORM_MINMAX)
  254.     filteredImg = np.uint8(filteredImg)
  255.     
  256.     points = cv2.reprojectImageTo3D(dispL,Q)
  257.  
  258.     cv2.imshow('filteredImg',filteredImg)
  259.     cv2.imshow('disparity', (dispL-min_disp)/num_disp)
  260.     
  261.     if cv2.waitKey(0) & 0xFF == ord(' '):
  262.         continue
  263.     if cv2.waitKey(0) & 0xFF == 27:
  264.         break
  265.  
  266. cv2.waitKey(0)
  267. cv2.destroyAllWindows()

matlab标定参数变xml

  1. function writeXML(stereoParams,file)
  2.  
  3. docNode = com.mathworks.xml.XMLUtils.createDocument('opencv_storage'); %创建xml文件对象
  4. docRootNode = docNode.getDocumentElement; %获取根节点
  5.  
  6. IntrinsicMatrixl = (stereoParams.CameraParameters1.IntrinsicMatrix)'; %相机内参矩阵
  7. RadialDistortionl = stereoParams.CameraParameters1.RadialDistortion; %相机径向畸变参数向量1*3
  8. TangentialDistortionl =stereoParams.CameraParameters1.TangentialDistortion; %相机切向畸变向量1*2
  9. Distortionl = [RadialDistortionl(1:2),TangentialDistortionl,RadialDistortionl(3)]; %构成opencv中的畸变系数向量[k1,k2,p1,p2,k3]
  11. IntrinsicMatrixr = (stereoParams.CameraParameters2.IntrinsicMatrix)'; %相机内参矩阵
  12. RadialDistortionr = stereoParams.CameraParameters2.RadialDistortion; %相机径向畸变参数向量1*3
  13. TangentialDistortionr =stereoParams.CameraParameters2.TangentialDistortion; %相机切向畸变向量1*2
  14. Distortionr = [RadialDistortionr(1:2),TangentialDistortionr,RadialDistortionr(3)]; %构成opencv中的畸变系数向量[k1,k2,p1,p2,k3]
  15.  
  16. R = stereoParams.RotationOfCamera2';
  17. T = stereoParams.TranslationOfCamera2;
  19. camera_matrix = docNode.createElement('lcamera-matrix'); %创建mat节点
  20. data = docNode.createElement('data');
  21. for i=1:3
  22.     for j=1:3
  23.         data.appendChild(docNode.createTextNode(sprintf('%.16f ',IntrinsicMatrixl(i,j))));
  24.     end
  25.     data.appendChild(docNode.createTextNode(sprintf('\n')));
  26. end
  27. camera_matrix.appendChild(data);
  28. docRootNode.appendChild(camera_matrix);
  30. camera_matrix = docNode.createElement('rcamera-matrix'); %创建mat节点
  31. data = docNode.createElement('data');
  32. for i=1:3
  33.     for j=1:3
  34.         data.appendChild(docNode.createTextNode(sprintf('%.16f ',IntrinsicMatrixr(i,j))));
  35.     end
  36.     data.appendChild(docNode.createTextNode(sprintf('\n')));
  37. end
  38. camera_matrix.appendChild(data);
  39. docRootNode.appendChild(camera_matrix);
  41. distortion = docNode.createElement('ldistortion');
  42. data = docNode.createElement('data');
  43. for i=1:5
  44.       data.appendChild(docNode.createTextNode(sprintf('%.16f ',Distortionl(i))));
  45. end
  46. distortion.appendChild(data);
  47. docRootNode.appendChild(distortion);
  49. distortion = docNode.createElement('rdistortion');
  50. data = docNode.createElement('data');
  51. for i=1:5
  52.       data.appendChild(docNode.createTextNode(sprintf('%.16f ',Distortionr(i))));
  53. end
  54. distortion.appendChild(data);
  55. docRootNode.appendChild(distortion);
  57. rM = docNode.createElement('R');
  58. data = docNode.createElement('data');
  59. for i=1:3
  60.     for j=1:3
  61.         data.appendChild(docNode.createTextNode(sprintf('%.16f ',R(i,j))));
  62.     end
  63.     data.appendChild(docNode.createTextNode(sprintf('\n')));
  64. end
  65. rM.appendChild(data);
  66. docRootNode.appendChild(rM);
  68. tM = docNode.createElement('T');
  69. data = docNode.createElement('data');
  70. for i = 1:3
  71.     data.appendChild(docNode.createTextNode(sprintf('%.16f ',T(i))));
  72. end
  73. tM.appendChild(data);
  74. docRootNode.appendChild(tM);
  76. xmlFileName = file;
  77. xmlwrite(xmlFileName,docNode);
  78. end

第二个版本，标定与立体校正匹配等全用cv

 

  1. import numpy as np
  2. import cv2
  3. import os
  4.  
  5. # 遍历图片
  6. def getFileList(dir,Filelist, ext=None):
  7.     """
  8.     获取文件夹及其子文件夹中文件列表
  9.     输入 dir：文件夹根目录
  10.     输入 ext: 扩展名
  11.     返回： 文件路径列表
  12.     """
  13.     newDir = dir
  14.     if os.path.isfile(dir):
  15.         if ext is None:
  16.             Filelist.append(dir)
  17.         else:
  18.             if ext in dir[-3:]:
  19.                 Filelist.append(dir)
  20.     
  21.     elif os.path.isdir(dir):
  22.         for s in os.listdir(dir):
  23.             newDir=os.path.join(dir,s)
  24.             getFileList(newDir, Filelist, ext)
  25.  
  26.     return Filelist
  27. # 计算内焦点世界坐标
  28. def calRealPoint(row,col,trueLength):
  29.     imgpoint = []
  30.     for rowIndex in range(0,col):
  31.         for colIndex in range(0,row):
  32.             imgpoint.append([colIndex*trueLength,rowIndex*trueLength,0])
  33.     imgpoint = np.array(imgpoint,np.float32)
  34.     return imgpoint
  35. # 画线
  36. def draw_line1(image1, image2):
  37.     # 建立输出图像
  38.     height = max(image1.shape[0], image2.shape[0])
  39.     width = image1.shape[1] + image2.shape[1]
  40.     
  41.     output = np.zeros((height, width,3), dtype=np.uint8)
  42.     output[0:image1.shape[0], 0:image1.shape[1]] = image1
  43.     output[0:image2.shape[0], image1.shape[1]:] = image2
  44.  
  45.     for k in range(15):
  46.         cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA)  # 直线间隔：100
  47.  
  48.     return output
  49. # 三维坐标计算
  50. def uvToXYZ(lx, ly, rx, ry):
  51.     mLeft = np.hstack([leftRotation, leftTranslation])
  52.     mLeftM = np.dot(MLS, mLeft)
  53.     mRight = np.hstack([R, T])
  54.     mRightM = np.dot(MRS, mRight)
  55.     # print(mLeft)
  56.     # print(mLeftM)
  57.     A = np.zeros(shape=(4, 3))
  58.     for i in range(0, 3):
  59.         A[0][i] = lx * mLeftM[2, i] - mLeftM[0][i]
  60.     for i in range(0, 3):
  61.         A[1][i] = ly * mLeftM[2][i] - mLeftM[1][i]
  62.     for i in range(0, 3):
  63.         A[2][i] = rx * mRightM[2][i] - mRightM[0][i]
  64.     for i in range(0, 3):
  65.         A[3][i] = ry * mRightM[2][i] - mRightM[1][i]
  66.     B = np.zeros(shape=(4, 1))
  67.     B[0][0] = mLeftM[0][3] - lx * mLeftM[2][3]
  68.     B[1][0] = mLeftM[1][3] - ly * mLeftM[2][3]
  69.     B[2][0] = mRightM[0][3] - rx * mRightM[2][3]
  70.     B[3][0] = mRightM[1][3] - ry * mRightM[2][3]
  71.     XYZ = np.zeros(shape=(3, 1))
  72.     # 采用最小二乘法求其空间坐标
  73.     cv2.solve(A, B, XYZ, cv2.DECOMP_SVD)
  74.     # print('坐标为：\n',XYZ)
  75.     return XYZ
  76. # 获取像素坐标
  77. def get_pixel(img,index):
  78.     gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
  79.  
  80.     _,corners = cv2.findChessboardCorners(gray,(row,col))
  81.     criteria = (cv2.TermCriteria_EPS+cv2.TermCriteria_MAX_ITER,30,0.01)
  82.     corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
  83.     return corners2[index].ravel()
  84. # 清空文件夹文件
  85. def del_file(path):
  86.     ls = os.listdir(path)
  87.     for i in ls:
  88.         c_path = os.path.join(path, i)
  89.         if os.path.isdir(c_path):
  90.             del_file(c_path)
  91.         else:
  92.             os.remove(c_path)
  93.  
  94. # 初始化
  95. del_file('./left_')
  96. del_file('./right_')
  97. del_file('./left_re')
  98. del_file('./right_re')
  99. leftRotation = np.array([[1, 0, 0],
  100.                          [0, 1, 0],
  101.                          [0, 0, 1]])
  102. leftTranslation = np.array([[0],
  103.                             [0],
  104.                             [0]])
  105. # 棋盘格内焦点个数横纵
  106. row = 9
  107. col = 6
  108. # 格子真实尺寸
  109. trueLength = 10
  110. # 图片后缀
  111. hz = 'jpg'
  112. # 遍历图片
  113. imglistL = getFileList('./left',[],hz)
  114. imglistR = getFileList('./right',[],hz)
  115. # 创建存储能被角点检测到的图片的文件夹
  116. folderL = './left_'
  117. folderR = './right_'
  118. if not os.path.exists(folderL):
  119.     os.mkdir(folderL)
  120. if not os.path.exists(folderR):
  121.     os.mkdir(folderR)
  122. # 首先检测图片角点检测是否正确
  123. for i in range(0,len(imglistL)):
  124.     ChessImaR = cv2.imread(imglistR[i],0)
  125.     ChessImaL = cv2.imread(imglistL[i],0)
  126.     # 角点检测
  127.     retR, cornersR = cv2.findChessboardCorners(ChessImaR,
  128.                                                (row,col),None)
  129.     retL, cornersL = cv2.findChessboardCorners(ChessImaL,
  130.                                                (row,col),None)
  131.     # 只要一对图片中有一张不能检测角点，这对图片便舍弃
  132.     if (retR == True) & (retL == True):
  133.         print('第%d对图片符合'%i)
  134.         # 同时存入另一个文件夹
  135.         cv2.imwrite(folderL+'/img'+str(i)+'.'+hz,ChessImaL)
  136.         cv2.imwrite(folderR+'/img'+str(i)+'.'+hz,ChessImaR)
  137.         # 画出检测的角点
  138.         cv2.drawChessboardCorners(ChessImaL, (row, col), cornersL, retL)
  139.         cv2.drawChessboardCorners(ChessImaR, (row, col), cornersR, retR)
  140.         cv2.imshow('imgL',ChessImaL)
  141.         cv2.imshow('imgR',ChessImaR)
  142.         if cv2.waitKey(0) & 0xFF == ord(' '):
  143.             continue
  144.         if cv2.waitKey(0) & 0xFF == 27:
  145.             break
  146.     else:
  147.         print('第%d对图片不符合,剔除'%i)
  148. #############################################################
  149. #############################################################
  150. # 对筛选后的图像进行标定，校正，匹配，求视差，最小二乘求解空间坐标
  151. # 亚像素的参数和立体标定的参数
  152. criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
  153. criteria_stereo = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
  154. # 遍历筛选后图片
  155. imglistL_ = getFileList(folderL,[],hz)
  156. imglistR_ = getFileList(folderR,[],hz)
  157. # 存储每对图片的世界坐标，z为0，其它根据格子实际长度计算
  158. objpoints = []
  159. # 分别存储左右图片的角点亚像素坐标   
  160. imgpointsR = []
  161. imgpointsL = []
  162. # 计算世界坐标下内角的坐标
  163. objp = calRealPoint(row,col,trueLength)
  164.  
  165. print('Starting calibration for the 2 cameras... ')
  166. for i in range(0,len(imglistL_)):   
  167.     ChessImaL_ = cv2.imread(imglistL_[i],0) 
  168.     ChessImaR_ = cv2.imread(imglistR_[i],0)   
  169.     retR_, cornersR_ = cv2.findChessboardCorners(ChessImaR_,
  170.                                                (row,col),None) 
  171.     retL_, cornersL_ = cv2.findChessboardCorners(ChessImaL_,
  172.                                                (row,col),None)
  173.     if (True == retR_) & (True == retL_):
  174.         # 亚像素
  175.         cornersR2_ = cv2.cornerSubPix(ChessImaR_,cornersR_,(11,11),(-1,-1),criteria)
  176.         cornersL2_ = cv2.cornerSubPix(ChessImaL_,cornersL_,(11,11),(-1,-1),criteria)
  177.         imgpointsR.append(cornersR2_)
  178.         imgpointsL.append(cornersL2_)
  179.         objpoints.append(objp)
  180.         # 画角点
  181.         cv2.drawChessboardCorners(ChessImaL_, (row, col), cornersL2_, retL_)
  182.         cv2.drawChessboardCorners(ChessImaR_, (row, col), cornersR2_, retR_)
  183.         print('第%d对图片'%i)
  184.         cv2.imshow('imgL',ChessImaL_)
  185.         cv2.imshow('imgR',ChessImaR_)
  186.         if cv2.waitKey(0) & 0xFF == ord(' '):
  187.             continue
  188.         if cv2.waitKey(0) & 0xFF == 27:
  189.             break
  190. # 类似先进行单目标定
  191. retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(objpoints,
  192.                                                         imgpointsR,
  193.                                                         ChessImaR_.shape[::-1],None,None)
  194. # 重投影误差计算
  195. for i in range(0,len(objpoints)):
  196.     err = 0
  197.     ChessImaR_1 = cv2.imread(imglistR_[i])
  198.     # 反向计算像素坐标，与亚像素角点坐标比较 ,计算的temp有两部分
  199.     temp = cv2.projectPoints(objpoints[i],rvecsR[i],tvecsR[i],mtxR,distR)
  200.     for j in range(0,temp[0].shape[0]):
  201.         err = err+np.linalg.norm(temp[0][j]-imgpointsR[i][j])
  202.     err = err/temp[0].shape[0]
  203.     print('右边第%d张重新投影与原角点亚像素平均误差为:%f\n'%(i,err))
  204.     cv2.drawChessboardCorners(ChessImaR_1, (row, col), temp[0],patternWasFound=True)
  205.     cv2.imshow('the_right',ChessImaR_1)
  206.     if cv2.waitKey(0) & 0xFF == ord(' '):
  207.         continue
  208.     if cv2.waitKey(0) & 0xFF == 27:
  209.         break
  210. ###############################################################################
  211. retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(objpoints,
  212.                                                         imgpointsL,
  213.                                                         ChessImaL_.shape[::-1],None,None)
  214. # 重投影误差计算
  215. for i in range(0,len(objpoints)):
  216.     err = 0
  217.     ChessImaL_1 = cv2.imread(imglistL_[i])
  218.     # 反向计算像素坐标，与亚像素角点坐标比较 ,计算的temp有两部分
  219.     temp = cv2.projectPoints(objpoints[i],rvecsL[i],tvecsL[i],mtxL,distL)
  220.     for j in range(0,temp[0].shape[0]):
  221.         err = err+np.linalg.norm(temp[0][j]-imgpointsL[i][j])
  222.     err = err/temp[0].shape[0]
  223.     print('左边第%d张重新投影与原角点亚像素平均误差为:%f\n'%(i,err))
  224.     cv2.drawChessboardCorners(ChessImaL_1, (row, col), temp[0],patternWasFound=True)
  225.     cv2.imshow('the_left',ChessImaL_1)
  226.     if cv2.waitKey(0) & 0xFF == ord(' '):
  227.         continue
  228.     if cv2.waitKey(0) & 0xFF == 27:
  229.         break
  230. print('Cameras Ready to use')
  231. #############################################################################
  232. # 立体标定及可选参数，具体查阅
  233. flags = 0
  234. flags |= cv2.CALIB_FIX_INTRINSIC
  235. #flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
  236. # flags |= cv2.CALIB_USE_INTRINSIC_GUESS
  237. #flags |= cv2.CALIB_FIX_FOCAL_LENGTH
  238. #flags |= cv2.CALIB_FIX_ASPECT_RATIO
  239. #flags |= cv2.CALIB_ZERO_TANGENT_DIST
  240. #flags |= cv2.CALIB_RATIONAL_MODEL
  241. #flags |= cv2.CALIB_SAME_FOCAL_LENGTH
  242. #flags |= cv2.CALIB_FIX_K3
  243. #flags |= cv2.CALIB_FIX_K4
  244. #flags |= cv2.CALIB_FIX_K5
  245. retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(objpoints,
  246.                                                           imgpointsL,
  247.                                                           imgpointsR,
  248.                                                           mtxL,
  249.                                                           distL,
  250.                                                           mtxR,
  251.                                                           distR,
  252.                                                           ChessImaR.shape[::-1],
  253.                                                           criteria_stereo,
  254.                                                           flags)
  255. print('L内参\n',MLS)
  256. print('L畸变\n',dLS)
  257. print('R内参\n',MRS)
  258. print('R畸变\n',dRS)
  259. print('旋转\n',R)
  260. print('平移\n',T)
  261. # 参数含义不知
  262. rectify_scale = 0 
  263. # Q重投影矩阵
  264. RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(MLS, dLS, MRS, dRS,
  265.                                                  ChessImaR.shape[::-1], R, T,
  266.                                                  rectify_scale,(0,0),alpha=-1)
  267. # 畸变校正
  268. Left_Stereo_Map = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
  269.                                              ChessImaR.shape[::-1], cv2.CV_16SC2)
  270. Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
  271.                                               ChessImaR.shape[::-1], cv2.CV_16SC2)
  272. # 参数设置参考官方例程
  273. window_size = 3
  274. min_disp = 16
  275. num_disp = 112-min_disp
  276. stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
  277.     numDisparities = num_disp,
  278.     blockSize = 16,
  279.     P1 = 8*3*window_size**2,
  280.     P2 = 32*3*window_size**2,
  281.     disp12MaxDiff = 1,
  282.     uniquenessRatio = 10,
  283.     speckleWindowSize = 100,
  284.     speckleRange = 32
  285. )
  286. # 使用滤波器降低噪声
  287. stereoR=cv2.ximgproc.createRightMatcher(stereo)
  288. # 过滤器参数设置
  289. lmbda = 80000
  290. sigma = 1.8
  291. visual_multiplier = 1.0 
  292. wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=stereo)
  293. wls_filter.setLambda(lmbda)
  294. wls_filter.setSigmaColor(sigma)
  295. # 创建保存极线校正的图片
  296. folderL_ = './left_re'
  297. folderR_ = './right_re'
  298. if not os.path.exists(folderL_):
  299.     os.mkdir(folderL_)
  300. if not os.path.exists(folderR_):
  301.     os.mkdir(folderR_)
  302. # 标定参数使用
  303. for i in range(0,len(imglistL_)):
  304.     frameR = cv2.imread(imglistR_[i])
  305.     frameL = cv2.imread(imglistL_[i])
  306.     # Rectify图片，极线校正
  307.     Left_nice = cv2.remap(frameL,Left_Stereo_Map[0],Left_Stereo_Map[1], cv2.INTER_AREA)
  308.     Right_nice = cv2.remap(frameR,Right_Stereo_Map[0],Right_Stereo_Map[1], cv2.INTER_AREA)
  309.     # 保存极线校正后的图片
  310.     cv2.imwrite(folderL_+'/img'+str(i)+'.'+hz,Left_nice)
  311.     cv2.imwrite(folderR_+'/img'+str(i)+'.'+hz,Right_nice)
  312.     # 显示极线校正的图片
  313.     res1 = draw_line1(Left_nice,Right_nice)
  314.     cv2.imshow('ln',Left_nice)
  315.     cv2.imshow('rn',Right_nice)
  316.     cv2.imshow('res1',res1)
  317.     # 计算距离三维坐标
  318.     u11,v11 = get_pixel(Left_nice,0)
  319.     u21,v21 = get_pixel(Right_nice,0)
  320.     u12,v12 = get_pixel(Left_nice,1)
  321.     u22,v22 = get_pixel(Right_nice,1)
  322.     p1 = uvToXYZ(u11,v11,u21,v21)
  323.     p2 = uvToXYZ(u12,v12,u22,v22)
  324.     print('p1坐标\n',p1)
  325.     print('p2坐标\n',p2)
  326.     print('距离\n',np.linalg.norm(p1-p2))
  327.     ################################################
  328.     grayR= cv2.cvtColor(Right_nice,cv2.COLOR_BGR2GRAY)
  329.     grayL= cv2.cvtColor(Left_nice,cv2.COLOR_BGR2GRAY)
  330.     # 计算深度，参考官方例程
  331.     dispL= stereo.compute(grayL,grayR).astype(np.float32)/ 16.0 
  332.     dispR= stereo.compute(grayR,grayL).astype(np.float32)/ 16.0
  333.     # 使用滤波器降噪,filteredimg为过滤后的深度图
  334.     filteredImg= wls_filter.filter(dispL,grayL,None,dispR)
  335.     filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, 
  336.                                 norm_type=cv2.NORM_MINMAX)
  337.     filteredImg = np.uint8(filteredImg)
  338.  
  339.     points = cv2.reprojectImageTo3D(dispL,Q)
  340.  
  341.     cv2.imshow('filteredImg',filteredImg)
  342.     cv2.imshow('disparity', (dispL-min_disp)/num_disp)
  343.     
  344.     if cv2.waitKey(0) & 0xFF == ord(' '):
  345.         continue
  346.     if cv2.waitKey(0) & 0xFF == 27:
  347.         break
  348.  
  349. cv2.waitKey(0)
  350. cv2.destroyAllWindows()

希望能帮到大家，最好有大佬能帮我解决一个问题，以上代码都是网上搜集自己整理，我也不知道是谁的了，就感谢大家所有人！

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Monodyee/article/detail/152814
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号