Problem with disparity map or stereo calibration

watermelon0guy
Sun, 12 May 2024 21:40:43 +0000
Python

Good day to everyone. I’ve been trying to solve the problem of surface reconstruction for some time now. I built a sparse point cloud using a simple triangulation. I still want a dense point cloud, so I decided to try disparity. However, I can’t get any good results. There is either noise in the image or there are some outlines of an object that, after conversion to a point cloud, simply turn out to be on the near and far planes (that is, they have either a minimum depth or a maximum). I came across the advice to abandon the chessboard, because when calibrating cameras separately, the orientation of the pattern has uncertainty (it is symmetrical with respect to one of the axes). And this can lead to poor stereo calibration. I changed the pattern to Charuco. The result is the same
Hence the question. I can’t figure out at what stage the error occurs. An important note is that my successful attempts with sparse point cloud were made on another camera (I just moved one camera from one position to another and back). I am currently using two Logitech C270 HD.
Charuko calibration:

def calibrate_with_charuco(images, charuco_board: cv.aruco.CharucoBoard):
    all_charuco_corners = []
    all_charuco_ids = []
    all_object_points = []
    all_image_points = []
    mask = []

    charuco_detector = cv.aruco.CharucoDetector(charuco_board)

    for img in images:
        gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
        curr_charuco_corners, curr_charuco_ids, marker_corners, marker_ids = charuco_detector.detectBoard(gray)
        if not isinstance(curr_charuco_corners, types.NoneType):
            curr_obj_points, curr_img_points = charuco_board.matchImagePoints(curr_charuco_corners, curr_charuco_ids)
            if curr_img_points.size == 0 or curr_obj_points.size == 0:
                continue

            all_charuco_corners.append(curr_charuco_corners)
            all_charuco_ids.append(curr_charuco_ids)
            all_image_points.append(curr_img_points)
            all_object_points.append(curr_obj_points)

    h, w = gray.shape

    ret, K, D, r_vecs, t_vecs = cv.calibrateCamera(all_object_points, all_image_points, (w, h), None, None)
    # K, roi = cv.getOptimalNewCameraMatrix(K, D, (w, h), 1, (w, h))

    mean_error = 0
    for i in range(len(all_object_points)):
        img_points_, _ = cv.projectPoints(all_object_points[i], r_vecs[i], t_vecs[i], K, D)
        error = cv.norm(all_image_points[i], img_points_, cv.NORM_L2) / len(img_points_)
        mean_error += error

    print("total error: {}".format(mean_error / len(all_object_points)))
    print(ret)

    return ret, K, D, r_vecs, t_vecs, all_object_points, all_image_points

Stereo calibration:

def calibrate_different_stereo_cameras(cam_1_images, cam_2_images, board: cv.aruco.CharucoBoard):
    default_image_size_1 = cam_1_images[0].shape[1::-1]
    default_image_size_2 = cam_2_images[0].shape[1::-1]
    image_size = default_image_size_1

    ret_1, K_1, D_1, _, _, obj_points_1, img_points_1 = calibration.calibrate_with_charuco(cam_1_images, board)
    ret_2, K_2, D_2, _, _, obj_points_2, img_points_2 = calibration.calibrate_with_charuco(cam_2_images, board)

    print(len(obj_points_1), len(obj_points_2))

    flags = 0
    flags |= cv.CALIB_FIX_INTRINSIC

    criteria_stereo = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    ret_stereo, K_1, D_1, K_2, D_2, R, T, E, F = cv.stereoCalibrate(obj_points_1, img_points_1, img_points_2, K_1, D_1,
                                                                    K_2, D_2, image_size, criteria_stereo, flags)
    print("Расстояние между камерами", np.linalg.norm(T))
    print(T)
    print(ret_stereo)

    rectify_scale = 1
    rect_L, rect_R, proj_matrix_L, proj_matrix_R, Q, roi_1, roi_2 = cv.stereoRectify(K_1, D_1,
                                                                                     K_2, D_2,
                                                                                     image_size, R, T,
                                                                                     rectify_scale, (0, 0))

    stereo_map_L = cv.initUndistortRectifyMap(K_1, D_1, rect_L, proj_matrix_L, image_size,
                                              cv.CV_16SC2)
    stereo_map_R = cv.initUndistortRectifyMap(K_2, D_2, rect_R, proj_matrix_R, image_size,
                                              cv.CV_16SC2)

    print("Saving parameters!")
    cv2_file = cv.FileStorage('stereo_map.xml', cv.FILE_STORAGE_WRITE)

    cv2_file.write('stereo_map_L_x', stereo_map_L[0])
    cv2_file.write('stereo_map_L_y', stereo_map_L[1])
    cv2_file.write('stereo_map_R_x', stereo_map_R[0])
    cv2_file.write('stereo_map_R_y', stereo_map_R[1])
    cv2_file.write('roi_1', roi_1)
    cv2_file.write('roi_2', roi_2)
    cv2_file.write('q', Q)

    cv2_file.release()


cam_1_images = helper.read_all_images('pics/cam_c270/calibration/left', extension='png')
cam_2_images = helper.read_all_images('pics/cam_c270/calibration/right', extension='png')

board = cv.aruco.CharucoBoard((5, 3), 40, 30, cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_50))
calibrate_different_stereo_cameras(cam_1_images, cam_2_images, board)

Jupyter notebook with depth map:

import cv2
import helper
import matplotlib.pyplot as plt
from IPython.display import display
from PIL import Image
import numpy as np
%load_ext autoreload
%autoreload 2
#%%
cv_file = cv2.FileStorage()
cv_file.open('stereo_map.xml', cv2.FileStorage_READ)

stereo_map_L_x = cv_file.getNode('stereo_map_L_x').mat()
stereo_map_L_y = cv_file.getNode('stereo_map_L_y').mat()
stereo_map_R_x = cv_file.getNode('stereo_map_R_x').mat()
stereo_map_R_y = cv_file.getNode('stereo_map_R_y').mat()
roi_1 = cv_file.getNode('roi_1').mat()
roi_2 = cv_file.getNode('roi_2').mat()

Q = cv_file.getNode('q').mat()
#%%
img_1 = cv2.imread('pics/cam_c270/test/left/1.png')
img_2 = cv2.imread('pics/cam_c270/test/right/1.png')
#%%
img_1 = cv2.remap(img_1, stereo_map_L_x, stereo_map_L_y, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)
img_2 = cv2.remap(img_2, stereo_map_R_x, stereo_map_R_y, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)

# img_1 = helper.crop_with_roi(img_1, roi_1)
# img_2 = helper.crop_with_roi(img_2, roi_2)
#%%
img_12 = np.concatenate((img_1, img_2), axis=1)
display(Image.fromarray(cv2.cvtColor(img_12, cv2.COLOR_BGR2RGB)))
cv2.imwrite('res/temp.jpg', img_12)
#%%
block_size = 6 # 7
win_size = 3
min_disp = 0 # 16
num_disp = 6 * 16

stereo = cv2.StereoSGBM_create(minDisparity= min_disp,
	numDisparities = num_disp,
	blockSize = block_size,
	uniquenessRatio = 10, # 15
	speckleWindowSize = 50, # 50
	speckleRange = 2, # 1
	disp12MaxDiff = 25, # 1
	preFilterCap=0, # 16
	P1 = 8 * 2 * win_size**2,#8*img_channels*block_size**2,
	P2 = 32 * 2 * win_size**2, #32*img_channels*block_size**2
	mode=cv2.StereoSGBM_MODE_SGBM)
# stereo = cv2.StereoBM.create(numDisparities=256, blockSize=5)
grey_1 = cv2.cvtColor(img_1, cv2.COLOR_BGR2GRAY)
grey_2 = cv2.cvtColor(img_2, cv2.COLOR_BGR2GRAY)
disparity = stereo.compute(grey_1, grey_2)
# norm_coeff = 255 / disparity.max()
# plt.imshow(disparity * norm_coeff / 255,'gray')
# plt.imshow(disparity, 'gray')
# plt.show()
# display(Image.fromarray(disparity))
disparity = disparity.astype(np.float32)
disparity = (disparity / 16.0 - min_disp) / num_disp

plt.imshow(disparity, 'gray_r')

# cv2.imshow("disp", disparity)
# cv2.waitKey(0)

disparity_map = disparity
#%%
# Convert disparity map to float32 and divide by 16 as show in the documentation
# print(disparity.dtype)
# disparity_map = np.float32(np.divide(disparity, 16.0))
print(disparity_map.dtype)

# Reproject points into 3D
points_3D = cv2.reprojectImageTo3D(disparity_map, Q, handleMissingValues=False)
# Get color of the reprojected points
colors = cv2.cvtColor(img_1, cv2.COLOR_BGR2RGB)

# Get rid of points with value 0 (no depth)
mask_map = disparity_map > disparity_map.min()

# Mask colors and points.
output_points = points_3D[mask_map]
output_colors = colors[mask_map]


# Function to create point cloud file
def create_point_cloud_file(vertices, colors, filename):
	colors = colors.reshape(-1,3)
	vertices = np.hstack([vertices.reshape(-1,3),colors])
	vertices = helper.remove_inf_vectors(vertices)
	ply_header = '''ply
		format ascii 1.0
		element vertex %(vert_num)d
		property float x
		property float y
		property float z
		property uchar red
		property uchar green
		property uchar blue
		end_header
		'''
	with open(filename, 'w') as f:
		f.write(ply_header %dict(vert_num=len(vertices)))
		np.savetxt(f,vertices,'%f %f %f %d %d %d')


output_file = 'pointCloud.ply'

# Generate point cloud file
create_point_cloud_file(output_points, output_colors, output_file)

Example of disparity input:

Result with this parameters:

block_size = 6 # 7
win_size = 3
min_disp = 0 # 16
num_disp = 6 * 16

stereo = cv2.StereoSGBM_create(minDisparity= min_disp,
	numDisparities = num_disp,
	blockSize = block_size,
	uniquenessRatio = 10, # 15
	speckleWindowSize = 50, # 50
	speckleRange = 2, # 1
	disp12MaxDiff = 25, # 1
	preFilterCap=0, # 16
	P1 = 8 * 2 * win_size**2,#8*img_channels*block_size**2,
	P2 = 32 * 2 * win_size**2, #32*img_channels*block_size**2
	mode=cv2.StereoSGBM_MODE_SGBM)

Resulted point cloud:

I do not know how to interpret the reprojection error, since I get different values: according to the formula from the examples and from the OpenCV function.
My calc: 0.0981043367307995
OpenCV: 0.2855580805727887
My calc: 0.08477536605253462
OpenCV: 0.24849477231650544
Stereo OpenCV: 0.5269908188135748

The distance between the cameras is 165.65127968890764(from T vector)
I realized that there seems to be an extra centimeter in the distance.

Thank you in advance for your help, I understand that I probably am not the first with such a question, but I have read all similar topics on the forum and have not found an answer to my question.

1 post - 1 participant