https://blog.csdn.net/qq_40700822/article/details/115765728
一、双目相机拍照后使用Matlab进行双目标定
必看： USB双目相机的具体标定过程：https://blog.csdn.net/qq_40700822/article/details/124251201?spm=1001.2014.3001.5501
主要参考：https://blog.csdn.net/dulingwen/article/details/98071584
感谢大佬的分享！！！(*≧ω≦)！！
二、标定后生成深度图，之后在进行测距（这里主要是对双目参数的解读）
1、导入相关库和相机的标定参数
首先导入需要的相关库以及双目相机标定的各项参数：
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import stereoconfig_040_2 #导入相机标定的参数
import pcl
import pcl.pcl_visualization
首先对导入的左右相机的图片进行预处理，一般可以削弱光照不均的影响，使得两个图片的曝光值差距缩小。
小知识：python-opencv读取的灰度图像是二维列表（数组）,彩色图像是三位列表（数组）。
.ndim返回的是数组的维度，返回的只有一个数，该数即表示数组的维度。
参考：https://blog.csdn.net/mokeding/article/details/17599585
#像素的访问和访问numpy中ndarray的方法完全一样
img[j,i] = 255 # 灰度图访问;j,i 分别表示图像的行和列 即 j * i 二维
#BGR 图像访问 彩色 BGR 图像 为 j * i * 3 三维
img[j,i,0] = 255 # 0 -- 为通道，指B通道
img[j,i,1] = 255 # 1 -- 为通道，指G通道
img[j,i,2] = 255 # 2 -- 为通道，指R通道
2、图像预处理
#预处理
def preprocess(img1, img2):
# 彩色图->灰度图
if(img1.ndim == 3): #判断是否为三维数组
img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) # 通过OpenCV加载的图像通道顺序是BGR
if(img2.ndim == 3):#判断是否为三维数组
img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# 直方图均衡
# 当然这里也可以按需对图像进行处理
img1 = cv2.equalizeHist(img1)
img2 = cv2.equalizeHist(img2)
return img1, img2
3、消除图像的畸变
cv.undistort()消除畸变函数参数含义
参考OpenCV官网：https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d
cv.undistort( src, cameraMatrix, distCoeffs[, dst[, newCameraMatrix]] ) -> dst
各个参数的含义：
参数名 含义
src 输入未畸变矫正的图片
dst 输出矫正之后的图片，与src输入的图片具有相同的大小和类型
cameraMatrix 输入相机的内参数矩阵 A = [ [fx, 0, cx], [0, fy, cy], [0, 0, 1] ]
distCoeffs 输入相机的畸变参数向量 （ (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy ] ] ] ] ) ）中的4, 5, 8, 12 或 14 个元素。如果向量为NULL /空，则假定失真系数为零。一般取标定后相机的[k1, k2, p1, p2, k3]
newCameraMatrix 畸变图像的相机矩阵。 默认情况下，它与cameraMatrix相同，但是您还可以使用其他矩阵来缩放和移动结果。
消除畸变函数的定义：
#消除畸变
def undistortion(image, camera_matrix, dist_coeff):
undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)
return undistortion_image
调用示例，不能直接使用（stereoconfig_040_2.py完整代码见文末）：
# 读取相机内参和外参
config = stereoconfig_040_2.stereoCamera()
i = 3
string = 'Val'
# 读取数据集的图片
iml = cv2.imread('./%sLeft%d.bmp' %(string,i) ) # 左图
imr = cv2.imread('./%sRight%d.bmp'%(string,i) ) # 右图
iml = undistortion(iml, config.cam_matrix_left, config.distortion_l)
imr = undistortion(imr, config.cam_matrix_right, config.distortion_r)
#cv2.undistort（）的dist_coeff参数的形式
# 左右相机畸变系数:[k1, k2, p1, p2, k3]
#config.distortion_l = np.array([[-0.0806, 0.3806, -0.0033, 0.0005148, -0.5229]])
#config.distortion_r = np.array([[-0.0485, 0.2200, -0.002, 0.0017, -0.2876]])
4、立体校正
# 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# @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
# 计算校正变换
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion,
(width, height), R, T, alpha=0)
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)
return map1x, map1y, map2x, map2y, Q
cv.stereoRectify()参数含义：
官网定义的形式：
cv.stereoRectify(
cameraMatrix1,
distCoeffs1,
cameraMatrix2,
distCoeffs2,
imageSize,
R,
T[, R1
[, R2
[, P1
[, P2
[, Q
[, flags
[, alpha
[, newImageSize]]]]]]]]
) -> R1, R2, P1, P2, Q, validPixROI1, validPixROI2
cv.stereoRectify()参数的含义：
参数名 含义
cameraMatrix1 第一个相机的内参数矩阵。
distCoeffs1 第一个相机的畸变参数。
cameraMatrix2 第二个相机的内参数矩阵。
distCoeffs2 第二个相机的畸变参数。
imageSize 双目相机标定的图像的尺寸大小。
R 从第一个摄像头的坐标系到第二个摄像头的坐标系的旋转矩阵，请参见stereoCalibrate。（输出旋转矩阵。连同平移矢量T一起，此矩阵将第一个摄像机坐标系中给定的点带到第二个摄像机坐标系中的点。用更专业的术语来说，R和T的元组执行的基础是从第一相机的坐标系到第二相机的坐标系的变化。由于其二元性，该元组等效于第一摄像机相对于第二摄像机坐标系的位置。）
T （输出转换向量，请参见上面的描述。）从第一个摄像机的坐标系到第二个摄像机的坐标系的平移向量，请参见stereoCalibrate。
R1 为第一个摄像机输出3×3立体矫正的变换（旋转矩阵）。 该矩阵将未校正的第一照相机的坐标系中给定的点带到校正的第一照相机的坐标系中的点。 用更多的专业术语来说就是，它执行了从未校正的第一摄像机的坐标系到校正了的第一摄像机的坐标系的基准的更改。
R2 为第二个摄像机输出3×3立体矫正的变换（旋转矩阵）。 该矩阵将未校正的第二照相机的坐标系中给定的点带到校正的第二照相机的坐标系中的点。 用更多的专业术语来说就是，它执行了从未校正的第二摄像机的坐标系到校正了的第二摄像机的坐标系的基准的更改。
P1 在第一个摄像机的新（校正）坐标系中输出3×4投影矩阵，即它将在校正后的第一摄像机坐标系中给定的点，投影到校正后的第一摄像机的图像中。
P2 在第二个摄像机的新（校正）坐标系中输出3×4投影矩阵，即它将在校正后的第二摄像机坐标系中给定的点，投影到校正后的第二摄像机的图像中。
Q 输出4×4视差深度映射矩阵（请参阅reprojectImageTo3D）。
flags 可能为零或CALIB_ZERO_DISPARITY的操作标志。如果设置了该标志，则该功能使每个摄像机的主要的像素点在校正后的视图中具有相同的像素坐标。并且，如果未设置该标志，则该功能仍可以在水平或垂直方向上移动图像（取决于对极线的方向），以最大化可用图像区域。
alpha 自由缩放参数。如果它是-1或缺省，则该函数将执行默认缩放。否则，该参数应在0到1之间。alpha = 0表示对校正的图像已经经过缩放和移动了，以便仅有效像素可见（在校正之后的非黑色区域）。 alpha = 1表示对校正后的图像进行抽取和移位，以便将来自摄像机的原始图像中的所有像素保留在校正后的图像中（不丢失任何源图像像素）。任何中间值（0～1）都会在这两种极端情况之间产生中间结果。
newImageSize 校正后的新图像的分辨率。应将相同的大小传递给initUndistortRectifyMap（请参阅OpenCV示例目录中的stereo_calib.cpp示例）。传递（0,0）（默认值）时，将其设置为原始imageSize。将其设置为较大的值可以帮助您保留原始图像中的细节，尤其是在径向变形较大的情况下。
validPixROI1 在所有像素均有效的已校正图像内的可选输出矩形。如果alpha = 0，则ROI覆盖整个图像。否则，它们可能会更小（请参见下图）。
validPixROI2 在所有像素均有效的已校正图像内的可选输出矩形。如果alpha = 0，则ROI覆盖整个图像。否则，它们可能会更小（请参见下图）。
cv.initUndistortRectifyMap()函数的定义：
cv.initUndistortRectifyMap(
cameraMatrix,
distCoeffs,
R,
newCameraMatrix,
size,
m1type[, map1
[, map2]
]
) -> map1, map2
cv.initUndistortRectifyMap()函数各个参数的含义：
参考OpenCV官网：
https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a
参数名 含义
cameraMatrix 相机的内参数矩阵。
distCoeffs 输入相机的畸变参数向量 （一般取标定后相机的[k1, k2, p1, p2, k3] ）。
R 对象空间中的可选的校正变换（3×3矩阵）。由stereoRectify计算的R1或R2可以在此处传递。如果矩阵为空，则假定身份转换。在cvInitUndistortMap中，R是一个单位矩阵。
newCameraMatrix 新相机内参数的矩阵。
size 未校正的图片的尺寸。
m1type 第一个输出映射的类型可以是CV_32FC1，CV_32FC2或CV_16SC2，请参见convertMaps。
map1 第一个输出图。
map2 第二个输出图。
5、视差计算
# 视差计算
def stereoMatchSGBM(left_image, right_image, down_scale=False):
# SGBM匹配参数设置
if left_image.ndim == 2:
img_channels = 1
else:
img_channels = 3
blockSize = 3
paraml = {'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': 1,
'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
}
# 构建SGBM对象
left_matcher = cv2.StereoSGBM_create(**paraml)
paramr = paraml
paramr['minDisparity'] = -paraml['numDisparities']
right_matcher = cv2.StereoSGBM_create(**paramr)
# 计算视差图
size = (left_image.shape[1], left_image.shape[0])
if down_scale == False:
disparity_left = left_matcher.compute(left_image, right_image)
disparity_right = right_matcher.compute(right_image, left_image)
else:
left_image_down = cv2.pyrDown(left_image)
right_image_down = cv2.pyrDown(right_image)
factor = left_image.shape[1] / left_image_down.shape[1]
disparity_left_half = left_matcher.compute(left_image_down, right_image_down)
disparity_right_half = right_matcher.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_left
disparity_right = factor * disparity_right
# 真实视差（因为SGBM算法得到的视差是×16的）
trueDisp_left = disparity_left.astype(np.float32) / 16.
trueDisp_right = disparity_right.astype(np.float32) / 16.
return trueDisp_left, trueDisp_right
Python-openCV 中cv2.StereoSGBM_create()参数的含义：
参考：https://docs.opencv.org/trunk/d2/d85/classcv_1_1StereoSGBM.html
参考OpenCV官网：https://docs.opencv.org/trunk/d2/d85/classcv_1_1StereoSGBM.html
cv2.StereoSGBM_create()的SGBM算法的定义：
cv2.StereoSGBM_create(
[，minDisparity
[，numDisparities
[，blockSize
[，P1
[，P2
[，disp12MaxDiff
[，preFilterCap
[，uniquenessRatio
[，speckleWindowSize
[，speckleRange
[，mode]]]]]]]]]]]]
)
各个参数的含义：
参数名 含义
minDisparity 最小可能的差异值。通常情况下，它是零，但有时整流算法可能会改变图像，所以这个参数需要作相应的调整。
numDisparities 最大差异减去最小差异。该值总是大于零。在当前的实现中，该参数必须可以被16整除。
blockSize 匹配的块大小。它必须是> = 1的奇数。通常情况下，它应该在3～11的范围内。
P1 控制视差平滑度的第一个参数。见下文。
P2 第二个参数控制视差平滑度。值越大，差异越平滑。P1是相邻像素之间的视差变化加或减1的惩罚。P2是相邻像素之间的视差变化超过1的惩罚。该算法需要P2> P1。请参见stereo_match.cpp示例，其中显示了一些相当好的P1和P2值（即分别设置为 P1 = 8 * number_of_image_channels * blockSize * blockSize和 P2 = 32 * number_of_image_channels * blockSize * blockSize）。
disp12MaxDiff 左右视差检查中允许的最大差异（以整数像素为单位）。将其设置为非正值以禁用检查。
preFilterCap 预滤波图像像素的截断值。该算法首先计算每个像素的x导数，并通过[-preFilterCap，preFilterCap]间隔剪切其值。结果值传递给Birchfield-Tomasi像素成本函数。
uniquenessRatio 最佳（最小）计算成本函数值应该“赢”第二个最佳值以考虑找到的匹配正确的百分比保证金。通常，5-15范围内的值就足够了。
speckleWindowSize 平滑视差区域的最大尺寸，以考虑其噪声斑点和无效。将其设置为0可禁用斑点过滤。否则，将其设置在50-200的范围内。
speckleRange 每个连接组件内的最大视差变化。如果你做斑点过滤，将参数设置为正值，它将被隐式乘以16.通常，1或2就足够好了。
mode 将其设置为StereoSGBM :: MODE_HH以运行全尺寸双通道动态编程算法。它将消耗O（W * H * numDisparities）字节，这对640×480立体声很大，对于HD尺寸的图片很大。默认情况下，它被设置为false。
Python-openCV 中cv2.pyrDown()中的参数和含义：
参考OpenCV官网：https://docs.opencv.org/master/d4/d86/group__imgproc__filter.html#gaf9bba239dfca11654cb7f50f889fc2ff
cv.pyrDown( src[, dst[, dstsize[, borderType]]] ) -> dst
参数名 含义
src 输入图像
dst 输出图像；与上面的输入图像具有相同的尺寸大小和类型
dstsize 输出图像的大小
borderType 像素外推方法，请参见BorderTypes（不支持BORDER_CONSTANT）
6、其他函数的参数以及含义
reprojectImageTo3D的定义：
cv.reprojectImageTo3D(
disparity,
Q
[, _3dImage
[, handleMissingValues
[, ddepth]]]
) -> _3dImage
cv2.reprojectImageTo3D()参数以及其含义：
参考官网：https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga1bc1152bd57d63bc524204f21fde6e02
参数名 含义
disparity 输入单通道8位无符号，16位有符号，32位有符号或32位浮点视差图像。 假定8位/ 16位带符号格式的值没有小数位。 如果视差是由StereoBM或StereoSGBM以及其他算法计算出的16位带符号格式，则应在此处使用之前将其除以16（并缩放为浮点数）。
_3dImage 输出与视差大小相同的3通道浮点图像。 _3dImage（x，y）的每个元素都包含根据视差图计算的点（x，y）的3D坐标。 如果使用通过stereoRectify获得的Q，则返回的点将在第一个摄像机的校正坐标系中表示。
Q 可以使用stereoRectify获得的4×4透视变换矩阵。
handleMissingValues 指示函数是否应处理缺失值（即未计算视差的点）。如果handleMissingValues = true，则具有与异常值相对应的最小视差的像素（请参见StereoMatcher :: compute）将转换为Z值非常大（当前设置为10000）的3D点。
ddepth 可选的输出数组深度。如果为-1，则输出图像将具有CV_32F深度。 ddepth也可以设置为CV_16S，CV_32S或CV_32F。
三、双目测距代码的实现
1、stereoconfig_040_2.py–相机标定的参数
import numpy as np
####################仅仅是一个示例###################################
# 双目相机参数
class stereoCamera(object):
def __init__(self):
# 左相机内参
self.cam_matrix_left = np.array([ [830.5873, -3.0662, 658.1007],
[ 0, 830.8116, 482.9859],
[ 0, 0, 1]
])
# 右相机内参
self.cam_matrix_right = np.array([ [830.4255, -3.5852, 636.8418],
[ 0, 830.7571, 476.0664],
[ 0, 0, 1]
])
# 左右相机畸变系数:[k1, k2, p1, p2, k3]
self.distortion_l = np.array([[-0.0806, 0.3806, -0.0033, 0.0005148, -0.5229]])
self.distortion_r = np.array([[-0.0485, 0.2200, -0.002, 0.0017, -0.2876]])
# 旋转矩阵
self.R = np.array([ [ 1, 0.0017, -0.0093],
[-0.0018, 1.0000, -0.0019],
[ 0.0093, 0.0019, 1.0000]
])
# 平移矩阵
self.T = np.array([[-119.9578], [0.1121], [-0.2134]])
# 焦距
self.focal_length = 859.367 # 默认值，一般取立体校正后的重投影矩阵Q中的 Q[2,3]
# 基线距离
self.baseline = 119.9578 # 单位：mm， 为平移向量的第一个参数（取绝对值）
2、dianyuntu.py–测距实现代码
pip install pcl
pip install pclpy0.11.0 -i https://pypi.tuna.tsinghua.edu.cn/simple
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import stereoconfig_040_2 #导入相机标定的参数
import pcl
import pcl.pcl_visualization
# 预处理
def preprocess(img1, img2):
# 彩色图->灰度图
if(img1.ndim == 3):#判断为三维数组
img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) # 通过OpenCV加载的图像通道顺序是BGR
if(img2.ndim == 3):
img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# 直方图均衡
img1 = cv2.equalizeHist(img1)
img2 = cv2.equalizeHist(img2)
return img1, img2
# 消除畸变
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
# 计算校正变换
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion,
(width, height), R, T, alpha=0)
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)
return map1x, map1y, map2x, map2y, Q
# 畸变校正和立体校正
def rectifyImage(image1, image2, map1x, map1y, map2x, map2y):
rectifyed_img1 = cv2.remap(image1, map1x, map1y, cv2.INTER_AREA)
rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_AREA)
return rectifyed_img1, rectifyed_img2
# 立体校正检验----画线
def draw_line(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
# 绘制等间距平行线
line_interval = 50 # 直线间隔：50
for k in range(height // line_interval):
cv2.line(output, (0, line_interval * (k + 1)), (2 * width, line_interval * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA)
return output
# 视差计算
def stereoMatchSGBM(left_image, right_image, down_scale=False):
# SGBM匹配参数设置
if left_image.ndim == 2:
img_channels = 1
else:
img_channels = 3
blockSize = 3
paraml = {'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': 1,
'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
}
# 构建SGBM对象
left_matcher = cv2.StereoSGBM_create(**paraml)
paramr = paraml
paramr['minDisparity'] = -paraml['numDisparities']
right_matcher = cv2.StereoSGBM_create(**paramr)
# 计算视差图
size = (left_image.shape[1], left_image.shape[0])
if down_scale == False:
disparity_left = left_matcher.compute(left_image, right_image)
disparity_right = right_matcher.compute(right_image, left_image)
else:
left_image_down = cv2.pyrDown(left_image)
right_image_down = cv2.pyrDown(right_image)
factor = left_image.shape[1] / left_image_down.shape[1]
disparity_left_half = left_matcher.compute(left_image_down, right_image_down)
disparity_right_half = right_matcher.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_left
disparity_right = factor * disparity_right
# 真实视差（因为SGBM算法得到的视差是×16的）
trueDisp_left = disparity_left.astype(np.float32) / 16.
trueDisp_right = disparity_right.astype(np.float32) / 16.
return trueDisp_left, trueDisp_right
# 将h×w×3数组转换为N×3的数组
def hw3ToN3(points):
height, width = points.shape[0:2]
points_1 = points[:, :, 0].reshape(height * width, 1)
points_2 = points[:, :, 1].reshape(height * width, 1)
points_3 = points[:, :, 2].reshape(height * width, 1)
points_ = np.hstack((points_1, points_2, points_3))
return points_
# 深度、颜色转换为点云
def DepthColor2Cloud(points_3d, colors):
rows, cols = points_3d.shape[0:2]
size = rows * cols
points_ = hw3ToN3(points_3d)
colors_ = hw3ToN3(colors).astype(np.int64)
# 颜色信息
blue = colors_[:, 0].reshape(size, 1)
green = colors_[:, 1].reshape(size, 1)
red = colors_[:, 2].reshape(size, 1)
rgb = np.left_shift(blue, 0) + np.left_shift(green, 8) + np.left_shift(red, 16)
# 将坐标+颜色叠加为点云数组
pointcloud = np.hstack((points_, rgb)).astype(np.float32)
# 删掉一些不合适的点
X = pointcloud[:, 0]
Y = -pointcloud[:, 1]
Z = -pointcloud[:, 2]
remove_idx1 = np.where(Z <= 0)
remove_idx2 = np.where(Z > 15000)
remove_idx3 = np.where(X > 10000)
remove_idx4 = np.where(X < -10000)
remove_idx5 = np.where(Y > 10000)
remove_idx6 = np.where(Y < -10000)
remove_idx = np.hstack((remove_idx1[0], remove_idx2[0], remove_idx3[0], remove_idx4[0], remove_idx5[0], remove_idx6[0]))
pointcloud_1 = np.delete(pointcloud, remove_idx, 0)
return pointcloud_1
# 点云显示
def view_cloud(pointcloud):
cloud = pcl.PointCloud_PointXYZRGBA()
cloud.from_array(pointcloud)
try:
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorACloud(cloud)
v = True
while v:
v = not (visual.WasStopped())
except:
pass
if __name__ == '__main__':
i = 3
string = 're'
# 读取数据集的图片
iml = cv2.imread('./%sLift%d.bmp' %(string,i) ) # 左图
imr = cv2.imread('./%sRight%d.bmp'%(string,i) ) # 右图
height, width = iml.shape[0:2]
print("width = %d \n" % width)
print("height = %d \n" % height)
# 读取相机内参和外参
config = stereoconfig_040_2.stereoCamera()
# 立体校正
map1x, map1y, map2x, map2y, Q = getRectifyTransform(height, width, config) # 获取用于畸变校正和立体校正的映射矩阵以及用于计算像素空间坐标的重投影矩阵
iml_rectified, imr_rectified = rectifyImage(iml, imr, map1x, map1y, map2x, map2y)
print("Print Q!")
print(Q)
# 绘制等间距平行线，检查立体校正的效果
line = draw_line(iml_rectified, imr_rectified)
cv2.imwrite('./%s检验%d.png' %(string,i), line)
# 消除畸变
iml = undistortion(iml, config.cam_matrix_left, config.distortion_l)
imr = undistortion(imr, config.cam_matrix_right, config.distortion_r)
# 立体匹配
iml_, imr_ = preprocess(iml, imr) # 预处理，一般可以削弱光照不均的影响，不做也可以
iml_rectified_l, imr_rectified_r = rectifyImage(iml_, imr_, map1x, map1y, map2x, map2y)
disp, _ = stereoMatchSGBM(iml_rectified_l, imr_rectified_r, True)
cv2.imwrite('./%s视差%d.png' %(string,i), disp)
# 计算像素点的3D坐标（左相机坐标系下）
points_3d = cv2.reprojectImageTo3D(disp, Q) # 可以使用上文的stereo_config.py给出的参数
#points_3d = points_3d
# 鼠标点击事件
def onMouse(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
print('点 (%d, %d) 的三维坐标 (%f, %f, %f)' % (x, y, points_3d[y, x, 0], points_3d[y, x, 1], points_3d[y, x, 2]))
dis = ( (points_3d[y, x, 0] ** 2 + points_3d[y, x, 1] ** 2 + points_3d[y, x, 2] **2) ** 0.5) / 1000
print('点 (%d, %d) 距离左摄像头的相对距离为 %0.3f m' %(x, y, dis) )
# 显示图片
cv2.namedWindow("disparity",0)
cv2.imshow("disparity", disp)
cv2.setMouseCallback("disparity", onMouse, 0)
# 构建点云--Point_XYZRGBA格式
pointcloud = DepthColor2Cloud(points_3d, iml)
# 显示点云
view_cloud(pointcloud)
cv2.waitKey(0)
cv2.destroyAllWindows()
3、代码打包下载地址
本人瞎写的代码，如有错误，还请见谅！
https://download.csdn.net/download/qq_40700822/18378679
4、查看实现的效果
直接使用python运行上边打包的代码的dianyuntu.py即可，如果报错，就是环境没有搭建好，自行解决吧。
PS. 我的摄像头的分辨率是1920*720，所以效果一般。
得到的视差图：
得到的点云图：
————————————————
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
原文链接：https://blog.csdn.net/qq_40700822/article/details/115765728
双目摄像头Matlab参数定标
https://blog.csdn.net/qq_40700822/article/details/124251201?spm=1001.2014.3001.5501
一、前期准备
1、安装好python3，可以在anaconda中安装python3。
2、一个合适的双目摄像头。
3、一台可以运行Matlab的电脑。
4、一张棋盘图（可A4打印，若效果不佳，则可A3打印）。
棋盘图如下图所示：需要测量小方框的边长（一般单位为毫米：mm）。
二、使用双目摄像头（左+右）拍摄棋盘图
1、注意事项
注意：
1、左、右摄像头图像中必须包含单独的完整的棋盘图。
2、可适当前后、左右、上下翻转棋盘图，在符合上述条件1的情况下。
3、拍摄左右双目的照片40～50张比较合适。
如图所示：
2、双目拍照代码（python）
take_photo.py内容如下：
import cv2
import sys
#引入库
cap = cv2.VideoCapture(1) #读取笔记本内置摄像头或者0号摄像头
i = 0
while True:
ret, frame = cap.read()
if (ret):
cv2.namedWindow("Video01",0) #创建一个名为Video01的窗口，0表示窗口大小可调
#cv2.resizeWindow("Video01",1280,720) ##创建一个名为Video01的窗口，设置窗口大小为 1920 * 1080 与上一个设置的 0 有冲突
cv2.imshow("Video01", frame)
#等待按键按下
c = cv2.waitKey(1) & 0xff
#r若按下w则保存一张照片
if c ==ord("w"):
cv2.imwrite("./val_001/%d.bmp" %i, frame) #自己设置拍摄的照片的存储位置
print("Save images %d succeed!" %i)
i+=1
#若按下Q键，则退出循环
if c == ord("q"):
break
#随时准备按q退出
cap.release()
#关掉所有窗口
cv2.destroyAllWindows()
注意：
1、运行take_photo.py前，注意设置左右照片的存储位置。
2、运行take_photo.py后，按下键盘上的“W”键拍摄一张照片。当拍摄的照片数量足够时，按下“Q”键退出程序运行。
3、程序退出后，打开存储照片的文件夹查看照片是否合适。
3、双目左右照片分割（python）
resize.py内容如下：
import numpy as np
import cv2
#img1 = cv2.imread(r"/Users/inbc/Desktop/zuo/Left1.bmp")
#img2 = cv2.imread(r"/Users/inbc/Desktop/you/Right1.bmp")
for i in range(0,7) :
#imgT = cv2.imdecode(np.fromfile('./images/%d.bmp' %i ,dtype=np.uint8), -1)
imgT = cv2.imdecode(np.fromfile('./val/%d.bmp' %i ,dtype=np.uint8), -1) #读取拍摄的左右双目照片
# cv2.imshow("zuo", img1[300:1200, 500:2000])
# cv2.imshow("you", img2[300:1200, 500:2000])
# cv2.waitKey(0)
#设置左右照片的存储位置
cv2.imwrite("./val/zuo/reLeft%d.bmp" %i ,imgT[0:720, 0:1280] )#imgL的第一个参数是图片高度像素范围，第二个参数是图片宽度的像素范围
cv2.imwrite("./val/you/reRight%d.bmp" %i ,imgT[0:720, 1280:2560] )
print("Resize images%d Fnished!" %i)
print(“Fnished All!!!”)
注意：
1、运行resize.py前，注意设置左、右照片的分别的存储位置。
2、运行resize.py后，终端打印”Fnished All!!!”表示分割完成。
3、程序退出后，打开存储照片的文件夹查看照片，是否分割完成，左摄像头照片存放在zuo，右摄像头照片存放在you。（文件名可自己更改）
三、Matlab双目参数标定
1、打开Matlab后，点开app，找到Stereo Camera Calibrator。如下图所示：打开后如图所示：
2、导入双目的左右照片到Stereo Camera CalibratorAPP。具体操作，如下图所示：
3、点击确认后就可以分析导入的左右的照片了。这个过程会自动剔除掉不合格（误差过大）的左右照片。
4、导入照片后就可以进行双目定标了。
导入左右照片后，如图所示：
5、设置双目相机的定标参数，如图所示。
Radial Distortion: 3 Coefifcients
Compute: Skew
Trangential Distortion
6、进行双目定标，并导出双目参数矩阵到Matlab中，进行下一步的处理。
四、双目参数提取
1、左、右相机内参数获取，注意参数矩阵的转置：
>> stereoParams.CameraParameters1.IntrinsicMatrix'#左相机参数
ans =
831.0889 -4.0855 659.4243
0 831.8663 487.3259
0 0 1.0000
>> stereoParams.CameraParameters2.IntrinsicMatrix'#右相机参数
ans =
831.1982 -3.5773 632.5308
0 832.1221 479.3084
0 0 1.0000
Matlab如图所示：
2、获取左右相机畸变系数。
注意： 左右相机畸变系数:[k1, k2, p1, p2, k3] ，顺序要正确。
# 左右相机畸变系数:[k1, k2, p1, p2, k3]
>> stereoParams.CameraParameters1.RadialDistortion
ans =
-0.0806 0.3806 -0.5229 #k1 k2 k3
>> stereoParams.CameraParameters1.TangentialDistortion
ans =
-0.0033 0.0005 #p1 p2
Matlab如图所示：
3、获取双目的旋转矩阵和平移矩阵，注意旋转矩阵的转置。
# 旋转矩阵
>> stereoParams.RotationOfCamera2'
ans =
1.0000 0.0017 -0.0093
-0.0018 1.0000 -0.0019
0.0093 0.0019 1.0000
# 平移矩阵
>> stereoParams.TranslationOfCamera2
ans =
-119.9578 0.1121 -0.2134
Matlab如图所示：
4、获取基线距离，单位：mm， 为平移向量的第一个参数（取绝对值）。
“`python
self.baseline = 119.9578 # 单位：mm， 为平移向量的第一个参数（取绝对值）
“`
至此，双目摄像头的参数就定标完了。
耐心一点，慢慢来总会成功的！！！
————————————————
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
原文链接：https://blog.csdn.net/qq_40700822/article/details/124251201
我的双目输出标定：
Standard Errors of Estimated Stereo Camera Parameters
-----------------------------------------------------
Camera 1 Intrinsics
-------------------
Focal length (pixels): [ 1557.0360 +/- 3.5529 1554.6651 +/- 3.6194 ]
Principal point (pixels):[ 934.5229 +/- 2.4870 725.9970 +/- 1.8253 ]
Skew: [ -0.5642 +/- 0.2435 ]
Radial distortion: [ -0.0521 +/- 0.0040 0.1695 +/- 0.0295 -0.1332 +/- 0.0685 ]
Tangential distortion: [ 0.0009 +/- 0.0003 -0.0025 +/- 0.0004 ]
Camera 1 Extrinsics
-------------------
Rotation vectors:
[ 0.1404 +/- 0.0014 -0.2401 +/- 0.0017 -0.0245 +/- 0.0003 ]
[ 0.0902 +/- 0.0015 -0.1148 +/- 0.0015 0.0815 +/- 0.0002 ]
[ 0.1252 +/- 0.0016 0.1732 +/- 0.0018 0.0860 +/- 0.0002 ]
[ 0.1272 +/- 0.0014 -0.3188 +/- 0.0016 -0.0038 +/- 0.0003 ]
[ -0.1238 +/- 0.0014 0.0813 +/- 0.0016 0.1334 +/- 0.0002 ]
[ -0.0096 +/- 0.0016 -0.1345 +/- 0.0018 -0.1244 +/- 0.0003 ]
[ 0.0091 +/- 0.0016 -0.0719 +/- 0.0017 -0.0394 +/- 0.0003 ]
[ -0.0103 +/- 0.0015 -0.3273 +/- 0.0017 0.0368 +/- 0.0003 ]
[ -0.0094 +/- 0.0016 -0.0829 +/- 0.0016 0.0072 +/- 0.0003 ]
[ -0.4454 +/- 0.0014 -0.4731 +/- 0.0016 -0.0582 +/- 0.0004 ]
[ -0.3218 +/- 0.0019 -0.3877 +/- 0.0017 -0.0660 +/- 0.0005 ]
[ 0.1001 +/- 0.0013 -0.2604 +/- 0.0017 -0.2103 +/- 0.0003 ]
[ -0.0395 +/- 0.0029 -0.1103 +/- 0.0022 -0.0281 +/- 0.0003 ]
[ 0.2228 +/- 0.0017 -0.1775 +/- 0.0020 -1.6743 +/- 0.0003 ]
[ -0.2014 +/- 0.0013 -0.6541 +/- 0.0015 0.0063 +/- 0.0005 ]
Translation vectors (mm):
[ -1.2974 +/- 0.7708 -141.0844 +/- 0.5631 475.4112 +/- 1.1521 ]
[ -31.0645 +/- 0.6866 -82.1405 +/- 0.5028 425.7039 +/- 1.0093 ]
[ 71.8146 +/- 0.7632 -162.2182 +/- 0.5620 470.0988 +/- 1.1743 ]
[ -57.8447 +/- 0.7099 -154.0450 +/- 0.5128 430.4074 +/- 1.0750 ]
[ 36.6614 +/- 0.7358 -57.2701 +/- 0.5417 461.4980 +/- 1.0773 ]
[ 28.1456 +/- 0.8723 45.5442 +/- 0.6426 544.3818 +/- 1.2791 ]
[ 15.7305 +/- 0.7876 105.5578 +/- 0.5784 485.7754 +/- 1.1772 ]
[ -56.7221 +/- 0.8205 5.1884 +/- 0.6023 511.1243 +/- 1.2029 ]
[ -24.3024 +/- 0.7441 107.8938 +/- 0.5449 457.0560 +/- 1.1144 ]
[ -135.9453 +/- 1.1810 48.2589 +/- 0.8615 728.4183 +/- 1.6883 ]
[ -87.2899 +/- 1.0914 196.4729 +/- 0.7916 657.7164 +/- 1.6548 ]
[ -21.4200 +/- 0.7104 -119.9380 +/- 0.5180 438.2329 +/- 1.0558 ]
[ -139.4850 +/- 1.1350 7.5592 +/- 0.8295 702.7607 +/- 1.6362 ]
[ 41.0943 +/- 0.9767 -76.5129 +/- 0.7219 611.1792 +/- 1.4357 ]
[ -111.3761 +/- 0.9381 21.4527 +/- 0.6818 576.3677 +/- 1.3849 ]
Camera 2 Intrinsics
-------------------
Focal length (pixels): [ 1558.3205 +/- 3.7873 1556.2857 +/- 3.7325 ]
Principal point (pixels):[ 886.4826 +/- 2.0573 711.1150 +/- 1.8086 ]
Skew: [ -0.1721 +/- 0.2234 ]
Radial distortion: [ -0.0387 +/- 0.0034 0.1205 +/- 0.0222 -0.0840 +/- 0.0455 ]
Tangential distortion: [ -0.0001 +/- 0.0003 -0.0020 +/- 0.0004 ]
Position And Orientation of Camera 2 Relative to Camera 1
---------------------------------------------------------
Rotation of camera 2: [ 0.9999 +/- 0.0009 0.0005 +/- 0.0009 -0.0125 +/- 0.0001 ]
Translation of camera 2 (mm): [ -120.3912 +/- 0.0487 -0.3378 +/- 0.0323 -0.6361 +/- 0.1601 ]
stereoParams =
stereoParameters with properties:
Parameters of Two Cameras
CameraParameters1: [1x1 cameraParameters]
CameraParameters2: [1x1 cameraParameters]
Inter-camera Geometry
RotationOfCamera2: [3x3 double]
TranslationOfCamera2: [-120.3912 -0.3378 -0.6361]
FundamentalMatrix: [3x3 double]
EssentialMatrix: [3x3 double]
Accuracy of Estimation
MeanReprojectionError: 0.2116
Calibration Settings
NumPatterns: 15
WorldPoints: [35x2 double]
WorldUnits: 'mm'
stereoParams =
stereoParameters with properties:
Parameters of Two Cameras
CameraParameters1: [1x1 cameraParameters]
CameraParameters2: [1x1 cameraParameters]
Inter-camera Geometry
RotationOfCamera2: [3x3 double]
TranslationOfCamera2: [-120.3912 -0.3378 -0.6361]
FundamentalMatrix: [3x3 double]
EssentialMatrix: [3x3 double]
Accuracy of Estimation
MeanReprojectionError: 0.2116
Calibration Settings
NumPatterns: 15
WorldPoints: [35x2 double]
WorldUnits: 'mm'
camera1:
camera2:
以下是我的实际测试参数数据：
通过在MATLAB Command Window 输入命令获取
1、左、右相机内参数获取，注意参数矩阵的转置：
左相机参数：
>> stereoParams.CameraParameters1.IntrinsicMatrix
ans =
1.0e+03 *
1.5570 0 0
-0.0006 1.5547 0
0.9345 0.7260 0.0010
右相机参数：
>> stereoParams.CameraParameters2.IntrinsicMatrix
ans =
1.0e+03 *
1.5583 0 0
-0.0002 1.5563 0
0.8865 0.7111 0.0010
2、获取左右相机畸变系数。
注意： 左右相机畸变系数:[k1, k2, p1, p2, k3] ，顺序要正确。
>> stereoParams.CameraParameters1.RadialDistortion
ans =
-0.0521 0.1695 -0.1332 #k1 k2 k3
>> stereoParams.CameraParameters1.TangentialDistortion
ans =
0.0009 -0.0025 #p1 p2
>> stereoParams.CameraParameters2.RadialDistortion
ans =
-0.0387 0.1205 -0.0840 #k1 k2 k3
>> stereoParams.CameraParameters2.TangentialDistortion
ans =
-0.0001 -0.0020 #p1 p2
3、获取双目的旋转矩阵和平移矩阵，注意旋转矩阵的转置。
>> stereoParams.RotationOfCamera2
ans =
0.9999 -0.0006 0.0125
0.0005 1.0000 0.0089
-0.0125 -0.0089 0.9999
>> stereoParams.TranslationOfCamera2
ans =
-120.3912 -0.3378 -0.6361
4、获取基线距离，单位：mm， 为平移向量的第一个参数（取绝对值）。
self.baseline=120.3912
至此，双目摄像头的参数就定标完了。
双目测试中配置文件使用
import numpy as np
####################仅仅是一个示例,是我的参数 2024.05.30 ##############################
# 双目相机参数
class stereoCamera(object):
def __init__(self):
# 左相机内参
'''
>> stereoParams.CameraParameters1.IntrinsicMatrix
ans =
1.0e+03 *
1.5570 0 0
-0.0006 1.5547 0
0.9345 0.7260 0.0010
'''
self.cam_matrix_left = np.array([[1557.0, -0.6, 934.5],
[0, 1554.7, 726.0],
[0, 0, 1]
])
# 右相机内参
'''
>> stereoParams.CameraParameters2.IntrinsicMatrix
ans =
1.0e+03 *
1.5583 0 0
-0.0002 1.5563 0
0.8865 0.7111 0.0010
'''
self.cam_matrix_right = np.array([[1558.3, -0.2, 886.5],
[0, 1556.3, 711.1],
[0, 0, 1]
])
# 左右相机畸变系数:[k1, k2, p1, p2, k3]
'''
**注意： 左右相机畸变系数:[k1, k2, p1, p2, k3] ，顺序要正确。**
>> stereoParams.CameraParameters1.RadialDistortion
ans =
-0.0521 0.1695 -0.1332 #k1 k2 k3
>> stereoParams.CameraParameters1.TangentialDistortion
ans =
0.0009 -0.0025 #p1 p2
>> stereoParams.CameraParameters2.RadialDistortion
ans =
-0.0387 0.1205 -0.0840 #k1 k2 k3
>> stereoParams.CameraParameters2.TangentialDistortion
ans =
-0.0001 -0.0020 #p1 p2
'''
self.distortion_l = np.array([[-0.0521, 0.1695, 0.0009, -0.0025, -0.1332]])
self.distortion_r = np.array([[-0.0387, 0.1205, -0.0001, -0.0020, -0.0840]])
# 旋转矩阵
'''
#注意旋转矩阵的转置。
>> stereoParams.RotationOfCamera2
ans =
0.9999 -0.0006 0.0125
0.0005 1.0000 0.0089
-0.0125 -0.0089 0.9999
'''
self.R = np.array([[0.9999, 0.0005, -0.0125],
[-0.0006, 1.0000, -0.0089],
[0.0125, 0.0089, 1.9999]
])
# 平移矩阵
'''
>> stereoParams.TranslationOfCamera2
ans =
-120.3912 -0.3378 -0.6361
'''
self.T = np.array([[-120.3912], [-0.3378], [-0.6361]])
# 焦距
'''
D:\AI\CV2\SHUANGMU>python dianyun.py
config.cam_matrix_left:
[[ 1.5570e+03 -6.0000e-01 9.3450e+02]
[ 0.0000e+00 1.5547e+03 7.2600e+02]
[ 0.0000e+00 0.0000e+00 1.0000e+00]]
Q:
[[ 1.00000000e+00 0.00000000e+00 0.00000000e+00 -9.03789375e+02]
[ 0.00000000e+00 1.00000000e+00 0.00000000e+00 -7.19628532e+02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.61991245e+03]
[ 0.00000000e+00 0.00000000e+00 8.30610631e-03 -0.00000000e+00]]
1557.0 1557.0 934.5 726.0
x: 31758.486 y: 43296.887 z: -195026.7
distance: 202283.55539983188
'''
# self.focal_length = 859.367 # 默认值，一般取立体校正后的重投影矩阵Q中的 Q[2,3]
self.focal_length = 1619.91245 # 默认值，一般取立体校正后的重投影矩阵Q中的 Q[2,3]
# 基线距离
self.baseline = 120.3912 # 单位：mm， 为平移向量的第一个参数（取绝对值）