当前位置:   article > 正文

【双目相机】双目摄像头测距并导出世界坐标数据进入txt【python】_根据双目rgb获得相机坐标系中的坐标

根据双目rgb获得相机坐标系中的坐标

1.双目测距,通过鼠标点击像素导出像素的世界坐标

代码如下,核心是threeD,图片内所有像素对应的世界坐标都储存在threeD

#从excel里读取数据
#144行fps帧率不准
import cv2
import pandas as pd
import numpy as np
import time
import random
import math

# -----------------------------------双目相机的基本参数---------------------------------------------------------
#   left_camera_matrix          左相机的内参矩阵
#   right_camera_matrix         右相机的内参矩阵
#
#   left_distortion             左相机的畸变系数    格式(K1,K2,P1,P2,0)
#   right_distortion            右相机的畸变系数
# -------------------------------------------------------------------------------------------------------------
# 左镜头的内参,如焦距
file_path = r'C:\out1.xlsx'   # r对路径进行转义,windows需要
raw_data = pd.read_excel(file_path)  # header=0表示第一行是表头,就自动去除了
#raw_data = pd.read_excel(file_path, header=0)
#print(raw_data)
data = raw_data.values     # 只提取表中信息
#print(data)
np.array([1,2,3,4,5,6,7,8,9],dtype='float32');
left_camera_matrix = np.array((data[4:7, 1:4]),dtype='float64');
right_camera_matrix = np.array((data[9:12, 1:4]),dtype='float64');
# 畸变系数,K1、K2、K3为径向畸变,P1、P2为切向畸变:[K1,K2,P1,P2,K3]
K1left= np.array((data[7:8, 1:2]),dtype='float64');
K2left= np.array((data[7:8, 2:3]),dtype='float64');
K3left= np.array((data[7:8, 3:4]),dtype='float64');
P1left= np.array((data[8:9, 1:2]),dtype='float64');
P2left= np.array((data[8:9, 2:3]),dtype='float64');
K1right= np.array((data[12:13, 1:2]),dtype='float64');
K2right= np.array((data[12:13, 2:3]),dtype='float64');
K3right= np.array((data[12:13, 3:4]),dtype='float64');
P1right= np.array((data[13:14, 1:2]),dtype='float64');
P2right= np.array((data[13:14, 2:3]),dtype='float64');
#畸变系数,K1、K2、K3为径向畸变,P1、P2为切向畸变:[K1,K2,P1,P2,K3]
left_distortion =np.hstack((K1left,K2left,P1left,P2left,K3left))
right_distortion =np.hstack((K1right,K2right,P1right,P2right,K3right))
# 旋转矩阵
R = np.array((data[1:4, 1:4]),dtype='float64');
# 平移矩阵
T = np.array(np.transpose(data[0:1, 1:4]),dtype='float64');
#T = np.array([-55.4164455,-0.307896388,-3.669759334])

size = (320, 180)
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
                                                                  right_camera_matrix, right_distortion, size, R,
                                                                  T)

# 校正查找映射表,将原始图像和校正后的图像上的点一一对应起来
left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)
print(Q)

# --------------------------鼠标回调函数---------------------------------------------------------
#   event               鼠标事件
#   param               输入参数
# -----------------------------------------------------------------------------------------------
def onmouse_pick_points(event, x, y, flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:
        threeD = param
        print('\n像素坐标 x = %d, y = %d' % (x, y))
        # print("世界坐标是:", threeD[y][x][0], threeD[y][x][1], threeD[y][x][2], "mm")
        print("世界坐标xyz 是:", threeD[y][x][0] / 1000.0, threeD[y][x][1] / 1000.0, threeD[y][x][2] / 1000.0, "m")
        distance = math.sqrt(threeD[y][x][0] ** 2 + threeD[y][x][1] ** 2 + threeD[y][x][2] ** 2)
        distance = distance / 1000.0  # mm -> m
        print("距离是:", distance, "m")
        with open ('C:/threeDD.txt', 'w') as outfile:
            for slice_2d in threeD:
                np.savetxt(outfile, slice_2d, fmt = '%f', delimiter = ',')

# 加载视频文件
capture = cv2.VideoCapture(0)
WIN_NAME = 'Deep disp'
cv2.namedWindow(WIN_NAME, cv2.WINDOW_AUTOSIZE)

# 读取视频
fps = 0.0
ret, frame = capture.read()
while ret:
    # 开始计时
    t1 = time.time()
    # 是否读取到了帧,读取到了则为True
    ret, frame = capture.read()
    # 切割为左右两张图片
    frame1 = frame[150:330, 0:320]
    frame2 = frame[150:330, 320:640]
    # 将BGR格式转换成灰度图片,用于畸变矫正
    imgL = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
    imgR = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)

    # 重映射,就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
    # 依据MATLAB测量数据重建无畸变图片,输入图片要求为灰度图
    img1_rectified = cv2.remap(imgL, left_map1, left_map2, cv2.INTER_LINEAR)
    img2_rectified = cv2.remap(imgR, right_map1, right_map2, cv2.INTER_LINEAR)

    # 转换为opencv的BGR格式
    imageL = cv2.cvtColor(img1_rectified, cv2.COLOR_GRAY2BGR)
    imageR = cv2.cvtColor(img2_rectified, cv2.COLOR_GRAY2BGR)

    # ------------------------------------SGBM算法----------------------------------------------------------
    #   blockSize                   深度图成块,blocksize越低,其深度图就越零碎,0<blockSize<10
    #   img_channels                BGR图像的颜色通道,img_channels=3,不可更改
    #   numDisparities              SGBM感知的范围,越大生成的精度越好,速度越慢,需要被16整除,如numDisparities
    #                               取16、32、48、64等
    #   mode                        sgbm算法选择模式,以速度由快到慢为:STEREO_SGBM_MODE_SGBM_3WAY、
    #                               STEREO_SGBM_MODE_HH4、STEREO_SGBM_MODE_SGBM、STEREO_SGBM_MODE_HH。精度反之
    # ------------------------------------------------------------------------------------------------------
    blockSize = 3
    img_channels = 3
    stereo = cv2.StereoSGBM_create(minDisparity=1,
                                   numDisparities=64,
                                   blockSize=blockSize,
                                   P1=8 * img_channels * blockSize * blockSize,
                                   P2=32 * img_channels * blockSize * blockSize,
                                   disp12MaxDiff=-1,
                                   preFilterCap=1,
                                   uniquenessRatio=10,
                                   speckleWindowSize=100,
                                   speckleRange=100,
                                   mode=cv2.STEREO_SGBM_MODE_HH)
    # 计算视差
    disparity = stereo.compute(img1_rectified, img2_rectified)

    # 归一化函数算法,生成深度图(灰度图)
    disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)

    # 生成深度图(颜色图)
    dis_color = disparity
    dis_color = cv2.normalize(dis_color, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
    dis_color = cv2.applyColorMap(dis_color, 2)

    # 计算三维坐标数据值
    threeD = cv2.reprojectImageTo3D(disparity, Q, handleMissingValues=True)
    # 计算出的threeD,需要乘以16,才等于现实中的距离
    threeD = threeD * 16

    # 鼠标回调事件
    cv2.setMouseCallback("depth", onmouse_pick_points, threeD)

    #完成计时,计算帧率
    fps = (fps + (1. / (time.time() - t1+0.1))) / 2
    frame = cv2.putText(frame, "fps= %.2f" % (fps), (0, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)

    cv2.imshow("depth", dis_color)
    cv2.imshow("left", frame1)
    cv2.imshow(WIN_NAME, disp)  # 显示深度图的双目画面,WIN_NAME = 'Deep disp'
    # 若键盘按下q则退出播放
    if cv2.waitKey(1) & 0xff == ord('q'):
        break

# 释放资源
capture.release()

# 关闭所有窗口
cv2.destroyAllWindows()

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159

测距时需用到matlab导出的相机标定参数,详见【双目相机】基于matlab的参数标定2-使用matlab标定

注意:使用之前必须新建立一个空的txt文件(如代码内的threeDD.txt),新建立的文件必须是空的(即不含任何内容)

使用此代码,在鼠标点击事件发生时即可记录此时刻的threeD内容

threeD即双目相机对应的图片的所有像素的世界坐标,通过其像素坐标(x,y)索引,索引公式:列数=320y+x+1)

2.将txt转化为excel索引数据

步骤如下所示
1.excel选择 打开
在这里插入图片描述2.点击要打开的txt文件,依次按照如下步骤进行
在这里插入图片描述在这里插入图片描述在这里插入图片描述完毕,即可在excel中查看数据

3.数据索引

如下图所示,假设python中点击像素(x,y)=(200,149)输出结果如下:
在这里插入图片描述按照公式:
索引公式:列数=320y+x+1
在这里插入图片描述

计算出结果在excel的47881行内
在这里插入图片描述

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

闽ICP备14008679号