Inconsistence of left-right image point reprojections after reprojectImageto3d

Question

everyone. I'm trying to triangulate some points (dense reconstruction) lying on a plane in a setup which involves two cameras. [Reference image]: https://imgur.com/gOps4vP and [The other image]: https://imgur.com/VIiH9Rv

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[0], maps1[1], cv2.INTER_LANCZOS4)
    img2_remap = cv2.remap(img2, maps2[0], maps2[1], cv2.INTER_LANCZOS4)

Result of the rectification: [Rectified reference image] https://drive.google.com/open?id=10VfgXrXFO3_lYqtO9qJXr17Dc6F1PuXU [The other one rectified] https://drive.google.com/open?id=13ZkeMiF5xEovGmX13LSQVaJ237hoJLX0

Now I call a function that recognize a known object in the images (target).

#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_mask = mp.maskPolygon(target_corners, img_scene1.shape[::-1]) # Output: mask of same dimension of the image

Target found (please note the highlighted corners): [Target found] https://imgur.com/QjYV8tp

Then I compute the disparity map using StereoSGBM. I'm interested in the computation of the target disparity only (I'll mask all the other points). With the Disparity map obtained and using the 4x4 projection Matrix Q given by stereoRectify, I perform the 3d reprojection of the disparity map.

    # 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

    target_disparity = target_mask*disp

    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.imshow('target_disparity', target_mask*(disp-min_disp)/num_disp)
    cv2.waitKey()
    cv2.destroyAllWindows()


    # Obtain matrix of the target 3D points starting from disparity image obtained from reprojectImageTo3D()
    mask_disp = disp > disp.min()
    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]))

    mask = mask_disp & mask_inf & mask_nan

    pts3D = points[mask]

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 camera has the same extrinsics of the original one. But this implies that the reconstructed 3d points will be expressed in a world reference that is not the world reference of the original camera!

So, applying the inverse rigid transform to the pts3D, we obtain a reconstruction in the original reference camera frame. (See code).

target3Dpts_hom = cv2.convertPointsToHomogeneous(target3Dpts)[:,0,:].T
    target3Dpts_hom = G.T @ target3Dpts_hom
    new_target3Dpts = cv2.convertPointsFromHomogeneous(target3Dpts_hom.T[:,np.newaxis,:])[:,0,:]

Please NOTE that if I don't perform this operation, the pt3D reprojected on the original cameras by means of their projection matrices will not correspond to the target points!

Check reconstruction via reprojection; Now, i can reproject the new_target3Dpts: Let me introduce the projection function that I call:

    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

Finally, the reprojections:

    #P_kin = K_kin[eye(3),0] # Originals MPPs of two cameras
    #P_rpi = K_rpi[R,t]

    m0 = proj.proj_dist(P_kin,dcoeffs_kin,new_points).astype('int32')

    for (x, y) in m0:

        x = int(x)
        y= int(y)

        cv2.circle(img_kin, (x, y), 2, (255, 255, 0), 4)

    cv2.namedWindow('frame1', cv2.WINDOW_NORMAL)
    cv2.resizeWindow('frame1', 800,450)
    cv2.imshow('frame1',img_kin)
    cv2.waitKey(0)

    m1 = proj.proj_dist(P_rpi,dcoeffs_rpi,new_points).astype('int32')
    img_rpi1 = img_rpi.copy()
    for (x, y) in m1:
        x = int(x)
        y = int(y)

    cv2.circle(img_rpi1, (x, y), 2, (255, 255, 0), 4)

    cv2.namedWindow('frame2', cv2.WINDOW_NORMAL)
    cv2.resizeWindow('frame2', 800,450)
    cv2.imshow('frame2',img_rpi1)
    cv2.waitKey(0)

But, while the reprojected points on the original reference camera are correct, this is not true for the second one....The points are simply translated, but I can't explain why.

Results: [First frame repj] https://imgur.com/S4lo9Wz [2nd frame repj. Error] https://imgur.com/y4igaEI

Any ideas? I will include all the code now. Thank you.

SM

smazzer smazzer · Accepted Answer · 2019-03-28T08:49:40

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[1]
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.

Inconsistence of left-right image point reprojections after reprojectImageto3d

1 Answers