相机参数
import cv2
import numpy as np
class CameraConfig:
class MatrixParameters:
def __init__(self, f_x, f_y, c_x, c_y):
self.f_x = f_x
self.f_y = f_y
self.c_x = c_x
self.c_y = c_y
def __init__(self, new_width=640, new_height=480):
# Left camera intrinsics
self._left_matrix = self.MatrixParameters(
f_x=416.841180253704,
f_y=416.465934495134,
c_x=338.485167779639,
c_y=230.419201769346,
)
self._left_distortion = np.array([
[-0.0170280933781798, 0.0643596519467521,
-0.00161785356900972, -0.00330684695473645, 0]
])
# Right camera intrinsics
self._right_matrix = self.MatrixParameters(
f_x=416.765094485395,
f_y=417.845058291483,
c_x=315.061245379892,
c_y=238.181766936442,
)
self._right_distortion = np.array([
[-0.0394089328586398, 0.131112076868352,
-0.00133793245429668, -0.00188957913931929, 0]
])
# Stereo extrinsics
self._T = np.array([[-120.326603502087],
[0.199732192805711],
[-0.203594457929446]])
self._R = np.array([
[0.999962872853149, 0.00187779299260463, -0.00840992323112715],
[-0.0018408858041373, 0.999988651353238, 0.00439412154902114],
[0.00841807904053251, -0.00437847669953504, 0.999954981430194]
])
# Image dimensions
self._original_width = 640
self._original_height = 480
self._new_width = new_width
self._new_height = new_height
@property
def width(self):
return self._new_width
@property
def height(self):
return self._new_height
@property
def T(self):
return self._T
@property
def R(self):
return self._R
@property
def left_distortion(self):
return self._left_distortion
@property
def right_distortion(self):
return self._right_distortion
@property
def left_matrix(self):
return self._get_matrix(self._left_matrix)
@property
def right_matrix(self):
return self._get_matrix(self._right_matrix)
def _get_matrix(self, matrix_params):
scale_x = self._new_width / self._original_width
scale_y = self._new_height / self._original_height
return np.array([
[matrix_params.f_x * scale_x, 0, matrix_params.c_x * scale_x],
[0, matrix_params.f_y * scale_y, matrix_params.c_y * scale_y],
[0, 0, 1]
])
if __name__ == '__main__':
# Example usage
camera = CameraConfig(new_width=1280, new_height=960)
print(camera)
print("\nLeft Camera Matrix:")
print(camera.left_matrix)
print("\nRight Camera Matrix:")
print(camera.right_matrix)
print("\nTranslation Vector:")
print(camera.translation_vector)
print("\nRotation Matrix:")
print(camera.rotation_matrix)
图像处理
import math
import cv2
import sys
import os
import open3d as o3d
import numpy as np
package_root = os.path.join(os.path.dirname(__file__), '..')
sys.path.append(package_root)
from script.camera_config import CameraConfig
def get_disparity(grayL, grayR, camera_param: CameraConfig):
size = (camera_param.width, camera_param.height)
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
camera_param.left_matrix, camera_param.left_distortion,
camera_param.right_matrix, camera_param.right_distortion,
size, camera_param.R, camera_param.T)
left_map1, left_map2 = cv2.initUndistortRectifyMap(
camera_param.left_matrix, camera_param.left_distortion, R1, P1, size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(
camera_param.right_matrix, camera_param.right_distortion, R2, P2, size, cv2.CV_16SC2)
# Ensure input images are single channel (grayscale)
if len(grayL.shape) > 2:
grayL = cv2.cvtColor(grayL, cv2.COLOR_BGR2GRAY)
if len(grayR.shape) > 2:
grayR = cv2.cvtColor(grayR, cv2.COLOR_BGR2GRAY)
img1_rectified = cv2.remap(grayL, left_map1, left_map2, cv2.INTER_LINEAR)
img2_rectified = cv2.remap(grayR, right_map1, right_map2, cv2.INTER_LINEAR)
# Convert to 8-bit if needed
img1_rectified = cv2.normalize(img1_rectified, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
img2_rectified = cv2.normalize(img2_rectified, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
numberOfDisparities = ((camera_param.width // 8) + 15) & -16
stereo = cv2.StereoBM_create(numDisparities=16, blockSize=9)
stereo.setROI1(validPixROI1)
stereo.setROI2(validPixROI2)
stereo.setPreFilterCap(31)
stereo.setBlockSize(15)
stereo.setMinDisparity(4)
stereo.setNumDisparities(numberOfDisparities)
stereo.setTextureThreshold(50)
stereo.setUniquenessRatio(15)
stereo.setSpeckleWindowSize(100)
stereo.setSpeckleRange(32)
stereo.setDisp12MaxDiff(1)
disparity = stereo.compute(img1_rectified, img2_rectified)
return disparity, validPixROI1, validPixROI2, Q
def get_xyz(imgL, imgR, class_name, top_xy, bottom_xy, camera_param: CameraConfig):
disparity, validPixROI1, _, Q = get_disparity(imgL, imgR, camera_param)
threeD = cv2.reprojectImageTo3D(disparity, Q, handleMissingValues=True) #计算三维坐标数据值
# 获取输入点 (x, y)
x1, y1 = top_xy
x2, y2 = bottom_xy
x, y = (x1+x2)/2, (y1+y2)/2 # 取中点作为参考
# 检查是否在有效 ROI 内
x_roi, y_roi, w_roi, h_roi = validPixROI1
if (x < x_roi) or (y < y_roi) or (x >= x_roi + w_roi) or (y >= y_roi + h_roi):
print(f"Warning: Point ({x}, {y}) is outside valid ROI {validPixROI1}")
return None
# 调整坐标到校正后的图像坐标系
x_rect = int(x - x_roi)
y_rect = int(y - y_roi)
# 检查是否在 threeD 范围内
if (x_rect < 0) or (y_rect < 0) or (x_rect >= threeD.shape[1]) or (y_rect >= threeD.shape[0]):
print(f"Error: Adjusted point ({x_rect}, {y_rect}) is outside threeD map")
return None
# 计算距离
x = threeD[y_rect, x_rect, 0]
y = threeD[y_rect, x_rect, 1]
z = threeD[y_rect, x_rect, 2]
local = (x, y, z)
print("x,y,z(mm): ", local)
cv2.rectangle(imgL, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2)
cv2.putText(imgL, class_name, (int(x1), int(y1) - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.imshow("left", imgL)
key = cv2.waitKey(1)
if key == ord("q"):
return
return local
def get_depth_img(imgL, imgR, camera_param: CameraConfig):
disparity, _, _, _ = get_disparity(imgL, imgR, camera_param)
disparity = disparity.astype(np.float32)
# 生成彩色视差图
disp_vis = cv2.normalize(disparity, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
disp_color = cv2.applyColorMap(disp_vis, cv2.COLORMAP_JET) # 使用JET颜色映射
# 显示彩色视差图
cv2.imshow("Disparity Color", disp_color)
key = cv2.waitKey(1)
if key == ord("q"):
return None
return disp_color
def get_pcd(imgL, imgR, camera_param: CameraConfig):
disparity_temp, _, _, Q = get_disparity(imgL, imgR, camera_param)
disparity = cv2.normalize(disparity_temp, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
numberOfDisparities = ((camera_param.width // 8) + 15) & -16
disparity[disparity < 0] = 0 # 剔除负视差
disparity[disparity > numberOfDisparities] = 0 # 剔除超出范围的视差
points_3d = cv2.reprojectImageTo3D(disparity, Q)
threshold = 5
gradient = cv2.Sobel(disparity, cv2.CV_32F, 1, 1, ksize=3)
mask = np.abs(gradient) > threshold # 阈值根据实际情况调整
sparse_points = points_3d[mask]
max_depth = 50*1000 # 设置最大有效深度
valid_mask = (sparse_points[:, 2] > 0) & (sparse_points[:, 2] < max_depth) # max_depth 是最大有效深度
sparse_points = sparse_points[valid_mask]
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(sparse_points) # sparse_points 是你的点云数据
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="Point Cloud (Non-Blocking)")
vis.add_geometry(pcd)
vis.run() # 非阻塞运行
return pcd