# Struggling with reprojectImageTo3d

Hi, everyone. I'm trying to triangulate some points lying on a plane in a setup which involves two cameras. link text link text

First of all, I solve the relative pose problem using the 5pts algorithm on the undistorted points for the Essential Matrix estimation, the I recover the pose. I'm using RANSAC.

Then, I rectify the stereo pairs the usual way:

R1, R2, Pn1, Pn2, Q, _, _ = cv2.stereoRectify(K1, dcoeffs1, K2, dcoeffs2,
img1.shape[::-1], R, t,
flags=cv2.CALIB_ZERO_DISPARITY,
alpha=-1)

# Compute the rigid transform that OpenCV apply to world points (USEFUL LATER)
# in order for the rectified reference camera to be K_new[I|0]
tn_1 = np.zeros((3,1)) # Cameras are never translated in the rectification

G1_rect = np.block([[R1, tn_1], [np.zeros((1,3)), 1.0]])
maps1   =   cv2.initUndistortRectifyMap(K1, dcoeffs1, R1, Pn1, (1920,1080), cv2.CV_32FC1)
maps2   =   cv2.initUndistortRectifyMap(K2, dcoeffs2, R2, Pn2, (1920,1080), cv2.CV_32FC1)
img1_remap = cv2.remap(img1, maps1, maps1, cv2.INTER_LANCZOS4)
img2_remap = cv2.remap(img2, maps2, maps2, cv2.INTER_LANCZOS4)


Result of the rectification:  #....
#Now call a function that recognize a known object in the images (target)
# Find target
target_corners, _ = dt.detectTarget(img_scene1, img_target, 0.5) # return 4 corners of the detected polygon
target_corners = target_corners[:,0,:]
# Compute mask for the target cutout:


Target found: https://imgur.com/QjYV8tp

# Compute disparity map
# https://docs.opencv.org/3.3.1/d2/d85/classcv_1_1StereoSGBM.html
window_size = 5
min_disp = 16
max_disp = 1024
num_disp = max_disp-min_disp # Deve essere divisibile per 16!

stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
numDisparities = num_disp,
blockSize = window_size,
P1 = 8*3*window_size**2,
P2 = 32*3*window_size**2,
disp12MaxDiff = 1,
uniquenessRatio = 10,
speckleWindowSize = 150,
speckleRange = 2
)

print('Calcolo SGBM della disparità...')
disp = stereo.compute(img_scene1, img_scene2).astype(np.float32) / 16.0

points = cv2.reprojectImageTo3D(target_disparity, Q)

# DEBUG:
cv2.namedWindow('scene1', cv2.WINDOW_NORMAL)
cv2.resizeWindow('scene1', 800,450)
cv2.imshow('scene1', img_scene1)
cv2.namedWindow('disparity', cv2.WINDOW_NORMAL)
cv2.resizeWindow('disparity', 800,450)
cv2.imshow('disparity', (disp-min_disp)/num_disp)
cv2.namedWindow('target_disparity', cv2.WINDOW_NORMAL)
cv2.resizeWindow('target_disparity', 800,450)
cv2.waitKey()
cv2.destroyAllWindows()

# Obtain matrix of the target 3D points starting from disparity image obtained from reprojectImageTo3D()
mask_inf = ~(np.isinf(points[:,:,0]) | np.isinf(points[:,:,1]) | np.isinf(points[:,:,2]))
mask_nan = ~(np.isnan(points[:,:,0]) | np.isnan(points[:,:,1]) | np.isnan(points[:,:,2]))



Now, i have 3d reconstructed the region of the images corresponding to the target. I noted that OpenCv, during camera rectification, apply a rigid transform to world points such that the reference original camera and the new (rectified) reference camera have the same extrinsics (R=eye(3) and t=[0,0,0]'). Infact, during rectification both cameras must be rotated, and I think OpenCV simply brings back the new cameras to a new reference such that the reference rectified ...

edit retag close merge delete

Sort by » oldest newest most voted

I solved the problem, which is not related with the reprojectImageto3D --that works fine--, but with this piece of code I've wrote and that I used to reproject the points onto the original frames:

def proj_dist(P, dcoeffs, M):
import numpy as np
import cv2

K, R, t,_,_,_,_ = cv2.decomposeProjectionMatrix(P)
rotv, _ = cv2.Rodrigues(R)

# Projection. Returns a (N,2) shaped array
m,_ = cv2.projectPoints(M,rotv,t[0:-1],K,dcoeffs)
m = m.squeeze()

return m


I've wrote my own function for points projection:

def proj(P, M, hom=0):
# proj(): Esegue la proiezione prospettica dei punti 3D M secondo la MPP P,
# sul piano immagine 2D di una camera pinhole.

import numpy as np

n = M.shape
M = np.concatenate((M, np.ones((1,n))))

# Proiezione
m = P @ M

m = m/m[2,:]

if hom !=1 :
# Passo a cartesiane
m = m[0:2,:]

return m


and the problem is solved! My function does not take in account for lens distortion. I'll further investigate the problem related with the projectPoints() OpenCV function.

more

Official site

GitHub

Wiki

Documentation