search

decadenza / DirectStereoRectification

"Rectifying Homographies for Stereo Vision: Analytical Solution for Minimal Distortion": algorithm to compute the optimal rectifying homographies that minimise perspective distortion.
GNU General Public License v3.0
26 stars 5 forks source link

Will this "DirectStereoRectification" algorithms fail with two cameras with completely different internal parameters? #6

Closed williamhyin closed 2 years ago

williamhyin commented 2 years ago

Hi

Thanks for your kindly help and guidance before.

I have a question about rectification with different fov(different internal parameters). One is :

[[258.13781734, 0., 954.22342933],
 [0., 258.11148133, 543.9914898],
 [0., 0., 1.]]

the another is ：

[[1.67048218e+03, 0.00000000e+00, 9.49693383e+02],
 [0.00000000e+00, 1.86404224e+03, 5.41460911e+02],
 [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]

also the have a wide baseline:

R = 
[[ 0.99938327, -0.01944831, -0.02923759], 
 [ 0.00775487,  0.93429903, -0.35640594], 
 [ 0.03424814,  0.3559594,   0.93387364]]

T = [ 0.01480007 -0.58130264 -1.53172435] when I use DirectStereoRectification: the perspective distortion is really big: 4077962.957242966

And the rectification failed.

Can you give me some suggestions?

Will this "DirectStereoRectification" algorithms fail with two cameras with completely different internal parameters? Should i scale to the similar focal length? Or is this related to the x_axis baseline length? too small failed?

Thanks

decadenza commented 2 years ago

Hi @williamhyin! You should have prepared the input like this (supposing the first camera is in the origin):

# Calibration data
A1 = np.array([[258.13781734, 0., 954.22342933], [0., 258.11148133, 543.9914898], [0., 0., 1.]])             # Left camera intrinsic matrix

A2 = np.array([[1.67048218e+03, 0.00000000e+00, 9.49693383e+02],[0.00000000e+00, 1.86404224e+03, 5.41460911e+02],[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])             # Right camera intrinsic matrix

RT1 = np.array([[ 1, 0, 0,  0],   # Left extrinsic parameters
                    [0,1,0,0],
                    [0,0,1,0]])
RT2 = np.array([[  0.99938327, -0.01944831, -0.02923759 , 0.01480007  ],   # Right extrinsic parameters
                    [ 0.00775487, 0.93429903, -0.35640594, -0.58130264],
                    [0.03424814, 0.3559594, 0.93387364, -1.53172435]])

Is it right? Please share the two camera resolutions and maybe the images (in original resolution) too, so I can run the code and double check you results.

If I interpreted the above correctly, the camera 2 is located in [-0.07088022, -0.00291994, 1.63685026], with a baseline of ~1.638. It seems like a vertical stereo setup to me, then this should be the problem. In this case you should consider the x-axis as vertical, the y-axis as horizontal (z-axis is still going outwards the camera). In other words, you should "tilt you head by 90°".

In fact, the algorithm is considered first for horizontal stereo rig:

IMG2NEW

But nothing prevents you to rotate everything and use it vertically. In order to do this, you must rotate your images and swap the x and y values in the calibration. It's maybe easier to redo calibration with rotated images.

The algorithm ensures the minimal possible distortion for that setup (this doesn't mean that there is no distortion). You don't have to rescale nor modify any value from calibration.

decadenza commented 2 years ago

Extra comment about vertical stereo rig.

If you have a vertical stereo setup, your epipolar lines would be like the following red lines (representative): image

Now, since rectification means to make all the epipolar lines horizontal (same y-coordinate), you are implicitly telling to distort the image so much to achieve this. While if you tilt your head by 90° you can see that the rectification can be achieved with much less distortion.

Hope that this representative example helps to understand.

williamhyin commented 2 years ago

Hi @williamhyin! You should have prepared the input like this (supposing the first camera is in the origin):

# Calibration data
A1 = np.array([[258.13781734, 0., 954.22342933], [0., 258.11148133, 543.9914898], [0., 0., 1.]])             # Left camera intrinsic matrix

A2 = np.array([[1.67048218e+03, 0.00000000e+00, 9.49693383e+02],[0.00000000e+00, 1.86404224e+03, 5.41460911e+02],[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])             # Right camera intrinsic matrix

RT1 = np.array([[ 1, 0, 0,  0],   # Left extrinsic parameters
                    [0,1,0,0],
                    [0,0,1,0]])
RT2 = np.array([[  0.99938327, -0.01944831, -0.02923759 , 0.01480007  ],   # Right extrinsic parameters
                    [ 0.00775487, 0.93429903, -0.35640594, -0.58130264],
                    [0.03424814, 0.3559594, 0.93387364, -1.53172435]])

Is it right? Please share the two camera resolutions and maybe the images (in original resolution) too, so I can run the code and double check you results.

If I interpreted the above correctly, the camera 2 is located in [-0.07088022, -0.00291994, 1.63685026], with a baseline of ~1.638. It seems like a vertical stereo setup to me, then this should be the problem. In this case you should consider the x-axis as vertical, the y-axis as horizontal (z-axis is still going outwards the camera). In other words, you should "tilt you head by 90°".

In fact, the algorithm is considered first for horizontal stereo rig: IMG2NEW But nothing prevents you to rotate everything and use it vertically. In order to do this, you must rotate your images and swap the x and y values in the calibration. It's maybe easier to redo calibration with rotated images.

The algorithm ensures the minimal possible distortion for that setup (this doesn't mean that there is no distortion). You don't have to rescale nor modify any value from calibration.

#left: resolution: origin(2880*1956) alrealy undistort to 1920*1080
A1 = np.array([[258.13781734, 0., 954.22342933],
                        [0., 258.11148133, 543.9914898],
                        [0., 0., 1.]])  
RT1 = np.array([[8.2937910687219969e-03, 4.4054654421143952e-01, -8.9769145891729785e-01, -1.1221412033580425e+00],
                    [9.9988425750439414e-01, 7.7970227576761942e-03, 1.3064380237974873e-02, -3.7819088840089776e-03],
                    [1.2754788300653771e-02, -8.9769591110769498e-01, -4.4043088737727182e-01, 9.8368441356520508e-02],
                    [0., 0., 0., 1.]])

#right: resolution: origin(1920*1080) alrealy undistort to 1920*1080

A2 = np.array([[1.67048218e+03, 0.00000000e+00, 9.49693383e+02],
                   [0.00000000e+00, 1.86404224e+03, 5.41460911e+02],
                   [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])  # Right camera intrinsic matrix

RT2 = np.array([[2.5967122092114721e-02, 7.3160908917485468e-01, -6.8122965966478066e-01, -1.7406952838516740e+00],
                    [9.9873399225065906e-01, 1.0382500507625871e-02, 4.9220081331396069e-02, 6.2863733933315610e-02],
                    [4.3082726159254334e-02, -6.8164532149788049e-01, -7.3041326274016827e-01, -1.4173031888024112e+00],
                    [0., 0., 0., 1.]])

#right to left: 
RT = np.array([[9.9938327322281428e-01, 7.7548699688642763e-03, 3.4248141471031855e-02, 4.2175700000000003e-02],
                  [-1.9448309545294230e-02, 9.3429902647673801e-01, 3.5595939709530083e-01, 1.0886300000000000e+00],
              [-2.9237586396353276e-02, -3.5640593586020092e-01, 9.3387363836084925e-01, 1.2236899999999999e+00],
              [0., 0., 0., 1.]])                

#left to right:
RT = np.array([[ 0.99938327, -0.01944831, -0.02923759,  0.01480007], 
            [ 0.00775487,  0.93429903, -0.35640594, -0.58130264],
            [ 0.03424814,  0.3559594,   0.93387364, -1.53172435],
            [ 0.          0.          0.          1.        ]])
decadenza commented 2 years ago

Could you please clarify what do you mean by "between using "RT1 &RT2" and "opencv RT convert to RT1 RT2" is different".

Also, please use code format in questions and share the code whenever is pertinent.

williamhyin commented 2 years ago

Could you please clarify what do you mean by "between using "RT1 &RT2" and "opencv RT convert to RT1 RT2" is different".

Also, please use code format in questions and share the code whenever is pertinent.

I have updated the above comment. Thanks for your reminder! And can you help me check why it did not work?

About using "RT1 &RT2" and "opencv RT convert to RT1 RT2" questions. please see this link. https://github.com/decadenza/DirectStereoRectification/issues/2#issuecomment-1031233316

In these questions, you have given a solution about deriving stereo extrinsic parameters.

Now I have both two camera extrinsic and one stereo extrinsic parameter. The effects of these two pairs are different when using your algorithms. Which one do you suggest?

decadenza commented 2 years ago

Something wrong in your code. RT1 and RT2 should be 3x4 matrices. Anyway, removing the last line, and using two separate extrinsic parameters, I get: Perspective distortion: 6899654.115043145 (which seems quite a lot, probably for the reason explained above).

By moving the world origin into the first camera, I obtain the exact same result (up to machine precision), as expected and discussed in the previous issue.

To move the extrinsics to the first camera you may refer to my other project, particularly to this function. I used the following code:

import simplestereo as ss

R, T = ss.utils.moveExtrinsicOriginToFirstCamera(RT1[:,:3], RT2[:,:3], RT1[:,3], RT2[:,3])
RT1 = np.array([[ 1, 0, 0,  0],   # Left extrinsic parameters
                [0, 1, 0, 0],
                [0, 0, 1, 0]])
RT2 = np.hstack((R, T))

So, to answer your question, the result must be the same whatever the origin of your system of reference. Paste your entire code and the images if you'd like more help.

williamhyin commented 2 years ago

Extra comment about vertical stereo rig.

If you have a vertical stereo setup, your epipolar lines would be like the following red lines (representative): image

Now, since rectification means to make all the epipolar lines horizontal (same y-coordinate), you are implicitly telling to distort the image so much to achieve this. While if you tilt your head by 90° you can see that the rectification can be achieved with much less distortion.

Hope that this representative example helps to understand.

Hi i am trying do vertical stereo setup with example images as you said. But it seems that the vertical rectification is not successful. Can you help me check it ? The following are code:

import cv2
import numpy as np

import rectification

if __name__ == "__main__":
    print("Direct Rectification EXAMPLE")

    # Load images
    img1 = cv2.imread("img/left.png")  # Left image
    img2 = cv2.imread("img/right.png")  # Right image

    def rotate(image, intrinsic_m, extrinsic_m):
        (h, w) = image.shape[:2]
        # calculate the center of the image
        rotated90 = cv2.rotate(image, cv2.cv2.ROTATE_90_CLOCKWISE)

        intrinsic90 = intrinsic_m.copy()
        intrinsic90[0, 0] = intrinsic_m[1, 1]
        intrinsic90[1, 1] = intrinsic_m[0, 0]
        intrinsic90[0, 2] = rotated90.shape[0] - intrinsic_m[1, 2]
        intrinsic90[1, 2] = intrinsic_m[0, 2]

        rotation_z = np.array([[np.cos(90), -np.sin(90), 0], [np.sin(90), np.cos(90), 0], [0, 0, 1]])
        extrinsic90 = rotation_z @ extrinsic_m[:3, :]
        return rotated90, intrinsic90, extrinsic90

    # Calibration data
    A1 = np.array([[960, 0, 960 / 2], [0, 960, 540 / 2], [0, 0, 1]])  # Left camera intrinsic matrix
    A2 = np.array([[960, 0, 960 / 2], [0, 960, 540 / 2], [0, 0, 1]])  # Right camera intrinsic matrix
    RT1 = np.array([[0.98920029, -0.11784191, -0.08715574, 2.26296163],  # Left extrinsic parameters
                    [-0.1284277, -0.41030705, -0.90285909, 0.15825593],
                    [0.07063401, 0.90430164, -0.42101002, 11.0683527],
                    [0.,0.,0.,1.]])
    RT2 = np.array([[0.94090474, 0.33686835, 0.03489951, 1.0174818],  # Right extrinsic parameters
                    [0.14616159, -0.31095025, -0.93912017, 2.36511779],
                    [-0.30550784, 0.88872361, -0.34181178, 14.08488464],
                    [0.,0.,0.,1.]])

    img1, A1, RT1 = rotate(img1, A1, RT1)
    img2, A2, RT2 = rotate(img2, A2, RT2)

    dims1 = img1.shape[::-1][1:]  # Image dimensions as (width, height)
    dims2 = img2.shape[::-1][1:]

    # Distortion coefficients
    # Empty because we're using digitally acquired images (no lens distortion).
    # See OpenCV distortion parameters for help.
    distCoeffs1 = np.array([])
    distCoeffs2 = np.array([])

    # 3x4 camera projection matrices
    Po1 = A1.dot(RT1)
    Po2 = A2.dot(RT2)

    # Fundamental matrix F is usually known from calibration, alternatively
    # you can get the Fundamental matrix from projection matrices
    F = rectification.getFundamentalMatrixFromProjections(Po1, Po2)

    # ANALYTICAL RECTIFICATION to get the **rectification homographies that minimize distortion**
    # See function dr.getAnalyticalRectifications() for details
    Rectify1, Rectify2 = rectification.getDirectRectifications(A1, A2, RT1, RT2, dims1, dims2, F)

    # Final rectified image dimensions (common to both images)
    destDims = dims1

    # Get fitting affine transformation to fit the images into the frame
    # Affine transformations do not introduce perspective distortion
    Fit1, Fit2 = rectification.getFittingMatrices(Rectify1, Rectify2, dims1, dims2, destDims=dims1)

    # Compute maps with OpenCV considering rectifications, fitting transformations and lens distortion
    # These maps can be stored and applied to rectify any image pair of the same stereo rig
    mapx1, mapy1 = cv2.initUndistortRectifyMap(A1, distCoeffs1, Rectify1.dot(A1), Fit1, destDims, cv2.CV_32FC1)
    mapx2, mapy2 = cv2.initUndistortRectifyMap(A2, distCoeffs2, Rectify2.dot(A2), Fit2, destDims, cv2.CV_32FC1)

    # Apply final transformation to images 
    img1_rect = cv2.remap(img1, mapx1, mapy1, interpolation=cv2.INTER_LINEAR);
    img2_rect = cv2.remap(img2, mapx2, mapy2, interpolation=cv2.INTER_LINEAR);

    # Draw a line as reference (optional)
    img1_rect = cv2.line(img1_rect, (0, int((destDims[1] - 1) / 2)), (destDims[0] - 1, int((destDims[1] - 1) / 2)),
                         color=(0, 0, 255), thickness=1)
    img2_rect = cv2.line(img2_rect, (0, int((destDims[1] - 1) / 2)), (destDims[0] - 1, int((destDims[1] - 1) / 2)),
                         color=(0, 0, 255), thickness=1)

    # Print some info
    perspDist = rectification.getLoopZhangDistortionValue(Rectify1, dims1) + rectification.getLoopZhangDistortionValue(
        Rectify2, dims2)
    print("Perspective distortion:", perspDist)

    # Show images
    # cv2.imshow('LEFT Rectified', img1_rect)
    # cv2.imshow('RIGHT Rectified', img2_rect)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()

    import matplotlib.pyplot as plt  # plt 

    plt.figure()
    plt.subplot(1, 2, 1)
    plt.imshow(img1_rect)
    plt.subplot(1, 2, 2)
    plt.imshow(img2_rect)
    plt.show()

Thanks

decadenza commented 2 years ago

Hello @williamhyin,

I appreciate your request, so even if it's not an issue, I'll try to explain with a clearer example.

1) Suppose we have a vertical stereo rig, with a top camera and a bottom one. The system is calibrated (we know intrinsics and extrinsics). Let's take the two pictures from a virtual scene I made for you... Say cheese:

top bottom

2) Ok, the algorithm doesn't care if the x-axis is actually horizontal. Let's tilt our head to the right (90°). What did we do? We rotated the world axes (rotation of 90° along z-axis). We'll see the two pictures rotated (reduced size for clarity): rotated_couple

3) Axes in the camera intrinsic and extrinsics must be also swapped.

4) Proceed with the algorithm as normal.

5) Rotate the rectified images back to the original orientation. TOP Rectified BOTTOM Rectified

6) That's all folks.

Here's the code I used for this. You should be able to download and use the first two images I linked above.

import cv2
import numpy as np

import rectification

def swap(m):
    s = m.copy()
    # focal length
    s[0, 0] = m[1, 1]
    s[1, 1] = m[0, 0]
    # principal point
    s[0, 2] = m[1, 2]
    s[1, 2] = m[0, 2]
    # skew factor
    s[0, 1] = m[1, 0]
    s[1, 0] = m[0, 1]

    return s

def rotateWorld90(extrinsic_m):
    a = np.pi/2
    rotation_z = np.array([[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]])
    extrinsic90 = rotation_z @ extrinsic_m
    return extrinsic90

if __name__ == "__main__":
    print("Direct Rectification EXAMPLE")

    # Load images
    img1 = cv2.imread("img/top.png")  # Top image
    img2 = cv2.imread("img/bottom.png")  # Bottom image

    # Calibration data VERTICAL STEREO
    A1 = np.array([[1.77777783e+03, 0.00000000e+00, 6.40000000e+02],
       [0.00000000e+00, 1.77777783e+03, 3.60000000e+02],
       [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])  # TOP camera intrinsic matrix
    A2 = np.array([[1.77777783e+03, 0.00000000e+00, 6.40000000e+02],
       [0.00000000e+00, 1.77777783e+03, 3.60000000e+02],
       [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])  # BOTTOM camera intrinsic matrix

    RT1 = np.array([[-1.00000036e+00,  4.81966929e-08,  1.04109733e-06,
        -9.54865165e-09],
       [-3.08187566e-07, -9.67931986e-01, -2.51212329e-01,
         1.91765144e-01],
       [ 9.95603841e-07, -2.51212329e-01,  9.67932343e-01,
         4.97697853e-02]])
    RT2 = np.array([[-0.95603412,  0.28449398, -0.07114441,  0.01241635],
       [-0.29285026, -0.91345638,  0.28255302, -0.32894552],
       [ 0.01539733,  0.29096505,  0.95660955,  0.09808378]])

    ### Tilt your head to the right side: the new y axis is the old x axis.
    # Rotate images
    img1 = cv2.rotate(img1, cv2.ROTATE_90_CLOCKWISE)
    img2 = cv2.rotate(img2, cv2.ROTATE_90_CLOCKWISE)
    # Swap intrinsics
    A1 = swap(A1)
    A2 = swap(A2)
    # Rotate world
    RT1 = rotateWorld90(RT1)
    RT2 = rotateWorld90(RT2)

    ### FROM HERE everything is left unchanged...
    dims1 = img1.shape[::-1][1:]  # Image dimensions as (width, height)
    dims2 = img2.shape[::-1][1:]

    # Distortion coefficients
    # Empty because we're using digitally acquired images (no lens distortion).
    # See OpenCV distortion parameters for help.
    distCoeffs1 = np.array([])
    distCoeffs2 = np.array([])

    # 3x4 camera projection matrices
    Po1 = A1.dot(RT1)
    Po2 = A2.dot(RT2)

    # Fundamental matrix F is usually known from calibration, alternatively
    # you can get the Fundamental matrix from projection matrices
    F = rectification.getFundamentalMatrixFromProjections(Po1, Po2)

    # ANALYTICAL RECTIFICATION to get the **rectification homographies that minimize distortion**
    # See function dr.getAnalyticalRectifications() for details
    Rectify1, Rectify2 = rectification.getDirectRectifications(A1, A2, RT1, RT2, dims1, dims2, F)

    # Final rectified image dimensions (common to both images)
    destDims = dims1

    # Get fitting affine transformation to fit the images into the frame
    # Affine transformations do not introduce perspective distortion
    Fit1, Fit2 = rectification.getFittingMatrices(Rectify1, Rectify2, dims1, dims2, destDims=dims1)

    # Compute maps with OpenCV considering rectifications, fitting transformations and lens distortion
    # These maps can be stored and applied to rectify any image pair of the same stereo rig
    mapx1, mapy1 = cv2.initUndistortRectifyMap(A1, distCoeffs1, Rectify1.dot(A1), Fit1, destDims, cv2.CV_32FC1)
    mapx2, mapy2 = cv2.initUndistortRectifyMap(A2, distCoeffs2, Rectify2.dot(A2), Fit2, destDims, cv2.CV_32FC1)

    # Apply final transformation to images 
    img1_rect = cv2.remap(img1, mapx1, mapy1, interpolation=cv2.INTER_LINEAR);
    img2_rect = cv2.remap(img2, mapx2, mapy2, interpolation=cv2.INTER_LINEAR);

    # Draw (horizontal) lines as reference (optional)
    for y in [385,572,700]:
        img1_rect = cv2.line(img1_rect, (0,y), (destDims[0] - 1, y),
                             color=(255, 0, 0), thickness=1)
        img2_rect = cv2.line(img2_rect, (0,y), (destDims[0] - 1, y),
                             color=(255, 0, 0), thickness=1)
    ### ... UNTIL HERE

    # Rotate images back to the expected position
    img1_rect = cv2.rotate(img1_rect, cv2.ROTATE_90_COUNTERCLOCKWISE)
    img2_rect = cv2.rotate(img2_rect, cv2.ROTATE_90_COUNTERCLOCKWISE)

    # Print some info
    perspDist = rectification.getLoopZhangDistortionValue(Rectify1, dims1) + rectification.getLoopZhangDistortionValue(
        Rectify2, dims2)
    print("Perspective distortion:", perspDist)

    # Show images
    cv2.imshow('TOP Rectified', img1_rect)
    cv2.imshow('BOTTOM Rectified', img2_rect)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

N.B. Note that np.sin and np.cos expect an angle in radiants!

williamhyin commented 2 years ago

Hello @williamhyin,

I appreciate your request, so even if it's not an issue, I'll try to explain with a clearer example.

  1. Suppose we have a vertical stereo rig, with a top camera and a bottom one. The system is calibrated (we know intrinsics and extrinsics). Let's take the two pictures from a virtual scene I made for you... Say cheese:

top bottom

  1. Ok, the algorithm doesn't care if the x-axis is actually horizontal. Let's tilt our head to the right (90°). What did we do? We rotated the world axes (rotation of 90° along z-axis). We'll see the two pictures rotated (reduced size for clarity): rotated_couple
  2. Axes in the camera intrinsic and extrinsics must be also swapped.
  3. Proceed with the algorithm as normal.
  4. Rotate the rectified images back to the original orientation. TOP Rectified BOTTOM Rectified
  5. That's all folks.

Here's the code I used for this. You should be able to download and use the first two images I linked above.

import cv2
import numpy as np

import rectification

def swap(m):
    s = m.copy()
    # focal length
    s[0, 0] = m[1, 1]
    s[1, 1] = m[0, 0]
    # principal point
    s[0, 2] = m[1, 2]
    s[1, 2] = m[0, 2]
    # skew factor
    s[0, 1] = m[1, 0]
    s[1, 0] = m[0, 1]

    return s

def rotateWorld90(extrinsic_m):
    a = np.pi/2
    rotation_z = np.array([[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]])
    extrinsic90 = rotation_z @ extrinsic_m
    return extrinsic90

if __name__ == "__main__":
    print("Direct Rectification EXAMPLE")

    # Load images
    img1 = cv2.imread("img/top.png")  # Top image
    img2 = cv2.imread("img/bottom.png")  # Bottom image

    # Calibration data VERTICAL STEREO
    A1 = np.array([[1.77777783e+03, 0.00000000e+00, 6.40000000e+02],
       [0.00000000e+00, 1.77777783e+03, 3.60000000e+02],
       [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])  # TOP camera intrinsic matrix
    A2 = np.array([[1.77777783e+03, 0.00000000e+00, 6.40000000e+02],
       [0.00000000e+00, 1.77777783e+03, 3.60000000e+02],
       [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])  # BOTTOM camera intrinsic matrix

    RT1 = np.array([[-1.00000036e+00,  4.81966929e-08,  1.04109733e-06,
        -9.54865165e-09],
       [-3.08187566e-07, -9.67931986e-01, -2.51212329e-01,
         1.91765144e-01],
       [ 9.95603841e-07, -2.51212329e-01,  9.67932343e-01,
         4.97697853e-02]])
    RT2 = np.array([[-0.95603412,  0.28449398, -0.07114441,  0.01241635],
       [-0.29285026, -0.91345638,  0.28255302, -0.32894552],
       [ 0.01539733,  0.29096505,  0.95660955,  0.09808378]])

    ### Tilt your head to the right side: the new y axis is the old x axis.
    # Rotate images
    img1 = cv2.rotate(img1, cv2.ROTATE_90_CLOCKWISE)
    img2 = cv2.rotate(img2, cv2.ROTATE_90_CLOCKWISE)
    # Swap intrinsics
    A1 = swap(A1)
    A2 = swap(A2)
    # Rotate world
    RT1 = rotateWorld90(RT1)
    RT2 = rotateWorld90(RT2)

    ### FROM HERE everything is left unchanged...
    dims1 = img1.shape[::-1][1:]  # Image dimensions as (width, height)
    dims2 = img2.shape[::-1][1:]

    # Distortion coefficients
    # Empty because we're using digitally acquired images (no lens distortion).
    # See OpenCV distortion parameters for help.
    distCoeffs1 = np.array([])
    distCoeffs2 = np.array([])

    # 3x4 camera projection matrices
    Po1 = A1.dot(RT1)
    Po2 = A2.dot(RT2)

    # Fundamental matrix F is usually known from calibration, alternatively
    # you can get the Fundamental matrix from projection matrices
    F = rectification.getFundamentalMatrixFromProjections(Po1, Po2)

    # ANALYTICAL RECTIFICATION to get the **rectification homographies that minimize distortion**
    # See function dr.getAnalyticalRectifications() for details
    Rectify1, Rectify2 = rectification.getDirectRectifications(A1, A2, RT1, RT2, dims1, dims2, F)

    # Final rectified image dimensions (common to both images)
    destDims = dims1

    # Get fitting affine transformation to fit the images into the frame
    # Affine transformations do not introduce perspective distortion
    Fit1, Fit2 = rectification.getFittingMatrices(Rectify1, Rectify2, dims1, dims2, destDims=dims1)

    # Compute maps with OpenCV considering rectifications, fitting transformations and lens distortion
    # These maps can be stored and applied to rectify any image pair of the same stereo rig
    mapx1, mapy1 = cv2.initUndistortRectifyMap(A1, distCoeffs1, Rectify1.dot(A1), Fit1, destDims, cv2.CV_32FC1)
    mapx2, mapy2 = cv2.initUndistortRectifyMap(A2, distCoeffs2, Rectify2.dot(A2), Fit2, destDims, cv2.CV_32FC1)

    # Apply final transformation to images 
    img1_rect = cv2.remap(img1, mapx1, mapy1, interpolation=cv2.INTER_LINEAR);
    img2_rect = cv2.remap(img2, mapx2, mapy2, interpolation=cv2.INTER_LINEAR);

    # Draw (horizontal) lines as reference (optional)
    for y in [385,572,700]:
        img1_rect = cv2.line(img1_rect, (0,y), (destDims[0] - 1, y),
                             color=(255, 0, 0), thickness=1)
        img2_rect = cv2.line(img2_rect, (0,y), (destDims[0] - 1, y),
                             color=(255, 0, 0), thickness=1)
    ### ... UNTIL HERE

    # Rotate images back to the expected position
    img1_rect = cv2.rotate(img1_rect, cv2.ROTATE_90_COUNTERCLOCKWISE)
    img2_rect = cv2.rotate(img2_rect, cv2.ROTATE_90_COUNTERCLOCKWISE)

    # Print some info
    perspDist = rectification.getLoopZhangDistortionValue(Rectify1, dims1) + rectification.getLoopZhangDistortionValue(
        Rectify2, dims2)
    print("Perspective distortion:", perspDist)

    # Show images
    cv2.imshow('TOP Rectified', img1_rect)
    cv2.imshow('BOTTOM Rectified', img2_rect)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

N.B. Note that np.sin and np.cos expect an angle in radiants!

I have found error that i used np.sin(90) for rotated extrinsic parameter calculation. Thank you very much for your careful explanation and guidance! You are the greatest author and also the friendliest author I have ever seen!

decadenza commented 2 years ago

Thank you for using this code. If you need stereo vision for your project, may I suggest to give a look at my other repository https://github.com/decadenza/SimpleStereo, which contains calibration and matching algorithms, and also this rectification algorithm and more. Cheers.