Styjun

I want to export my pointcloud from python into a .ply file, so I can analyse it in meshlab/matlab from a disparity map

First I load the images and the calibration Matrix. Then I rectify the Images before creating the disparity map using the SBGM algorithm

Here is my Code:

import numpy as np
import cv2

 #load unrectified images
unimgR =cv2.imread("R.jpg")
unimgL =cv2.imread("L.jpg")

#load calibration from calibration file
calibration = np.load(r"C:UsersXXXPycharmProjectsrectifyTest3_OpenCV_Rectified.npz", allow_pickle=False) # load variables from calibration file
imageSize = tuple(calibration["imageSize"])
leftMatrix = calibration["leftMatrix"]
leftDist = calibration["leftDist"]
leftMapX = calibration["leftMapX"]
leftMapY = calibration["leftMapY"]
leftROI = tuple(calibration["leftROI"])
rightMatrix = calibration["rightMatrix"]
rightDist = calibration["rightDist"]
rightMapX = calibration["rightMapX"]
rightMapY = calibration["rightMapY"]
rightROI = tuple(calibration["rightROI"])
disparityToDepthMap = calibration["disparityToDepthMap"]

# Rectify images (including monocular undistortion)
imgL = cv2.remap(unimgL, leftMapX, leftMapY, cv2.INTER_LINEAR)
imgR = cv2.remap(unimgR, rightMapX, rightMapY, cv2.INTER_LINEAR)

# SGBM Parameters 
window_size = 15 # wsize default 3; 5; 7 for SGBM reduced size image; 15 for SGBM full size image (1300px and above); 5 Works nicely
left_matcher = cv2.StereoSGBM_create(
 minDisparity=0,
 numDisparities=160, # max_disp has to be dividable by 16 f. E. HH 192, 256
 blockSize=5,
 P1=8 * 3 * window_size ** 2,
 # wsize default 3; 5; 7 for SGBM reduced size image; 15 for SGBM full size image (1300px and above); 5 Works nicely
 P2=32 * 3 * window_size ** 2,
 disp12MaxDiff=1,
 uniquenessRatio=15,
 speckleWindowSize=0,
 speckleRange=2,
 preFilterCap=63,
 mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY
)
right_matcher = cv2.ximgproc.createRightMatcher(left_matcher)

# FILTER Parameters
lmbda = 80000
sigma = 1.2
visual_multiplier = 1.0

# Weighted least squares filter to fill sparse (unpopulated) areas of the disparity map
 # by aligning the images edges and propagating disparity values from high- to low-confidence regions
wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=left_matcher)
wls_filter.setLambda(lmbda)
wls_filter.setSigmaColor(sigma)

# Get depth information/disparity map using SGBM
displ = left_matcher.compute(imgL, imgR) # .astype(np.float32)/16
dispr = right_matcher.compute(imgR, imgL) # .astype(np.float32)/16
displ = np.int16(displ)
dispr = np.int16(dispr)

filteredImg = wls_filter.filter(displ, imgL, None, dispr) # important to put "imgL" here!!!
filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, norm_type=cv2.NORM_MINMAX);
filteredImg = np.uint8(filteredImg)

# Calculate 3D point cloud
points = cv2.reprojectImageTo3D(filteredImg,disparityToDepthMap) / 420 # needs to be divided by 420 to obtain metric values (80 without normalization)
print('...shape of the pointcloud:', pointCloud.shape)

cv2.imshow('Disparity Map', filteredImg)
cv2.waitKey()
cv2.destroyAllWindows()

I found this code on the Internet:
https://github.com/opencv/opencv/blob/master/samples/python/stereo_match.py

I want it to modify for my code
So I added:

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

'''
def write_ply(fn, verts, colors):
 verts = verts.reshape(-1, 3)
 colors = colors.reshape(-1, 3)
 verts = np.hstack([verts, colors])
 with open(fn, 'wb') as f:
 f.write((ply_header % dict(vert_num=len(verts))).encode('utf-8'))
 np.savetxt(f, verts, fmt='%f %f %f %d %d %d ')

colors = cv.cvtColor(imgL, cv.COLOR_BGR2RGB)
mask = displ> displ.min()
out_points = points[mask]
out_colors = colors[mask]
out_fn = 'out.ply'
write_ply('out.ply', out_points, out_colors)
print('%s saved' % 'out.ply')

but it only gives me an empty file ( if I want matlab to read it, it gives me the warning: "Warning: Not all points defined in the header could be loaded. ").

What did I wrong?

Sorry if it is a obvious fault, I am really a newbie into python