zed4.py

'''
///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2018, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

/*****************************************************************************************
 ** This sample demonstrates how to capture stereo images and calibration parameters    **
 ** from the ZED camera with OpenCV without using the ZED SDK.                          **
 *****************************************************************************************/
'''

# open /Applications/Python\ 3.7/Install\ Certificates.command
# https://stackoverflow.com/questions/50236117/scraping-ssl-certificate-verify-failed-error-for-http-en-wikipedia-org

import numpy as np
import os
import configparser
import sys
import cv2
import wget

left_point = (10,10)
right_point = (200,200)
left_point_dis = (10,10)
right_point_dis = (200,200)

left_point_np = [10,10]
right_point_np = [200,200]

def download_calibration_file(serial_number) :

    directory = os.getcwd()
    if os.name == 'nt' :
        hidden_path = os.getenv('APPDATA') + '\\Stereolabs\\settings\\'
    else :
#        hidden_path = '/usr/local/zed/settings/'
        hidden_path = directory + '/settings/'
    # serial_number = "000019387"
    calibration_file = hidden_path + 'SN' + str(serial_number) + '.conf'

    if os.path.isfile(calibration_file) == False:
        url = 'http://calib.stereolabs.com/?SN='
        filename = wget.download(url=url+str(serial_number), out=calibration_file)

        if os.path.isfile(calibration_file) == False:
            print('Invalid Calibration File')
            return ""

    return calibration_file

def init_calibration(calibration_file, image_size) :

    cameraMarix_left = cameraMatrix_right = map_left_y = map_left_x = map_right_y = map_right_x = np.array([])

    config = configparser.ConfigParser()
    config.read(calibration_file)

    check_data = True
    resolution_str = ''
    if image_size.width == 2208 :
        resolution_str = '2K'
    elif image_size.width == 1920 :
        resolution_str = 'FHD'
    elif image_size.width == 1280 :
        resolution_str = 'HD'
    elif image_size.width == 672 :
        resolution_str = 'VGA'
    else:
        resolution_str = 'HD'
        check_data = False

    T_ = np.array([-float(config['STEREO']['Baseline'] if 'Baseline' in config['STEREO'] else 0),
                   float(config['STEREO']['TY_'+resolution_str] if 'TY_'+resolution_str in config['STEREO'] else 0),
                   float(config['STEREO']['TZ_'+resolution_str] if 'TZ_'+resolution_str in config['STEREO'] else 0)])


    left_cam_cx = float(config['LEFT_CAM_'+resolution_str]['cx'] if 'cx' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_cy = float(config['LEFT_CAM_'+resolution_str]['cy'] if 'cy' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_fx = float(config['LEFT_CAM_'+resolution_str]['fx'] if 'fx' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_fy = float(config['LEFT_CAM_'+resolution_str]['fy'] if 'fy' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_k1 = float(config['LEFT_CAM_'+resolution_str]['k1'] if 'k1' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_k2 = float(config['LEFT_CAM_'+resolution_str]['k2'] if 'k2' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_p1 = float(config['LEFT_CAM_'+resolution_str]['p1'] if 'p1' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_p2 = float(config['LEFT_CAM_'+resolution_str]['p2'] if 'p2' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_p3 = float(config['LEFT_CAM_'+resolution_str]['p3'] if 'p3' in config['LEFT_CAM_'+resolution_str] else 0)
    left_cam_k3 = float(config['LEFT_CAM_'+resolution_str]['k3'] if 'k3' in config['LEFT_CAM_'+resolution_str] else 0)


    right_cam_cx = float(config['RIGHT_CAM_'+resolution_str]['cx'] if 'cx' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_cy = float(config['RIGHT_CAM_'+resolution_str]['cy'] if 'cy' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_fx = float(config['RIGHT_CAM_'+resolution_str]['fx'] if 'fx' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_fy = float(config['RIGHT_CAM_'+resolution_str]['fy'] if 'fy' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_k1 = float(config['RIGHT_CAM_'+resolution_str]['k1'] if 'k1' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_k2 = float(config['RIGHT_CAM_'+resolution_str]['k2'] if 'k2' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_p1 = float(config['RIGHT_CAM_'+resolution_str]['p1'] if 'p1' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_p2 = float(config['RIGHT_CAM_'+resolution_str]['p2'] if 'p2' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_p3 = float(config['RIGHT_CAM_'+resolution_str]['p3'] if 'p3' in config['RIGHT_CAM_'+resolution_str] else 0)
    right_cam_k3 = float(config['RIGHT_CAM_'+resolution_str]['k3'] if 'k3' in config['RIGHT_CAM_'+resolution_str] else 0)

    R_zed = np.array([float(config['STEREO']['RX_'+resolution_str] if 'RX_' + resolution_str in config['STEREO'] else 0),
                      float(config['STEREO']['CV_'+resolution_str] if 'CV_' + resolution_str in config['STEREO'] else 0),
                      float(config['STEREO']['RZ_'+resolution_str] if 'RZ_' + resolution_str in config['STEREO'] else 0)])

    R, _ = cv2.Rodrigues(R_zed)
    cameraMatrix_left = np.array([[left_cam_fx, 0, left_cam_cx],
                         [0, left_cam_fy, left_cam_cy],
                         [0, 0, 1]])

    cameraMatrix_right = np.array([[right_cam_fx, 0, right_cam_cx],
                          [0, right_cam_fy, right_cam_cy],
                          [0, 0, 1]])

    distCoeffs_left = np.array([[left_cam_k1], [left_cam_k2], [left_cam_p1], [left_cam_p2], [left_cam_k3]])

    distCoeffs_right = np.array([[right_cam_k1], [right_cam_k2], [right_cam_p1], [right_cam_p2], [right_cam_k3]])

    T = np.array([[T_[0]], [T_[1]], [T_[2]]])
    R1 = R2 = P1 = P2 = np.array([])

    R1, R2, P1, P2 = cv2.stereoRectify(cameraMatrix1=cameraMatrix_left,
                                       cameraMatrix2=cameraMatrix_right,
                                       distCoeffs1=distCoeffs_left,
                                       distCoeffs2=distCoeffs_right,
                                       R=R, T=T,
                                       flags=cv2.CALIB_ZERO_DISPARITY,
                                       alpha=0,
                                       imageSize=(image_size.width, image_size.height),
                                       newImageSize=(image_size.width, image_size.height))[0:4]

    map_left_x, map_left_y = cv2.initUndistortRectifyMap(cameraMatrix_left, distCoeffs_left, R1, P1, (image_size.width, image_size.height), cv2.CV_32FC1)
    map_right_x, map_right_y = cv2.initUndistortRectifyMap(cameraMatrix_right, distCoeffs_right, R2, P2, (image_size.width, image_size.height), cv2.CV_32FC1)

    cameraMatrix_left = P1
    cameraMatrix_right = P2

    return cameraMatrix_left, cameraMatrix_right, map_left_x, map_left_y, map_right_x, map_right_y

class Resolution :
    width = 2208 #1280
    height = 1242 #720


def onMouse(event, x, y, flags, param):
    global left_point
    global right_point
    global left_point_dis
    global right_point_dis
    global left_point_np
    global right_point_np
    if event == cv2.EVENT_LBUTTONDOWN:
        # cv2.circle(lastImage, (x, y), 3, (255, 0, 0), -1)
        left_point_dis = (x,y)
        left_point = (x,y)
        left_point_np = [x,y]
        # print("left_point", left_point)
    else :
        right_point_dis = (x,left_point[1])
        right_point = (x-2208,left_point[1])
        right_point_np = [x-2208,left_point[1]]
        # print("right_point", right_point)

#make sure variables are numpy arrays
def DLT(P1, P2, point1, point2):

    A = [point1[1]*P1[2,:] - P1[1,:],
         P1[0,:] - point1[0]*P1[2,:],
         point2[1]*P2[2,:] - P2[1,:],
         P2[0,:] - point2[0]*P2[2,:]
        ]
    A = np.array(A).reshape((4,4))
    #print('A: ')
    #print(A)

    B = A.transpose() @ A
    from scipy import linalg
    U, s, Vh = linalg.svd(B, full_matrices = False)

    #print('Triangulated point: ')
    #print(Vh[3,0:3]/Vh[3,3])
    return Vh[3,0:3]/Vh[3,3]

def main() :

    # if len(sys.argv) == 1 :
    #     print('Please provide ZED serial number')
    #     exit(1)

    # Open the ZED camera
    cap = cv2.VideoCapture(0)
    if cap.isOpened() == 0:
        exit(-1)

    image_size = Resolution()
    image_size.width = 2208 #1280
    image_size.height = 1242 #720

    # Set the video resolution to HD720
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, image_size.width*2)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, image_size.height)

    # serial_number = int(sys.argv[1])
    serial_number = "000019387"
    calibration_file = download_calibration_file(serial_number)
    if calibration_file  == "":
        exit(1)
    print("Calibration file found. Loading...")

    camera_matrix_left, camera_matrix_right, map_left_x, map_left_y, map_right_x, map_right_y = init_calibration(calibration_file, image_size)

    print("camera_matrix_left ",camera_matrix_left)
    print("camera_matrix_right ",camera_matrix_right)
    while True :
        # Get a new frame from camera
        retval, frame = cap.read()
        # Extract left and right images from side-by-side
        left_right_image = np.split(frame, 2, axis=1)
        # Display images
#        cv2.imshow("left RAW", left_right_image[0])

        left_rect = cv2.remap(left_right_image[0], map_left_x, map_left_y, interpolation=cv2.INTER_LINEAR)
        right_rect = cv2.remap(left_right_image[1], map_right_x, map_right_y, interpolation=cv2.INTER_LINEAR)

        # print("right_rect ",right_rect.shape[0],right_rect.shape[1])
#        cv2.imshow("left RECT", left_rect)
#        cv2.imshow("right RECT", right_rect)
        #https://answers.opencv.org/question/175912/how-to-display-multiple-images-in-one-window/
        numpy_horizontal = np.hstack((left_rect, right_rect))
        
        numpy_horizontal = cv2.line(numpy_horizontal, left_point_dis, right_point_dis,(255,255,0),2)

# https://github.com/stereolabs/zed-examples/issues/44
# https://github.com/stereolabs/zed-opencv-native/blob/f7e17b6368ecbc432c0ca0d64be4586e88db8799/src/calibration.cpp#L74-L175
  
        point_4d_hom = cv2.triangulatePoints(camera_matrix_left, camera_matrix_right, left_point, right_point)
        good_pts_mask = np.where(point_4d_hom[3]!= 0)[0]
        point_4d = point_4d_hom / point_4d_hom[3] 
        # print("point_4d_hom ",point_4d_hom)
        # print("point_4d ",point_4d)
        _p3d = DLT(camera_matrix_left, camera_matrix_right, left_point_np, right_point_np) #calculate 3d position of keypoint

        # print("point_4d_hom ",point_4d_hom)
        print("_p3d ",_p3d)
  

        myStr =  "left: "  + str(left_point[0]) + ", " + str(left_point[1]) + "\n"
        myStr +=  "right: "  + str(right_point[0]) + ", " + str(right_point[1]) + "\n"
        myStr +=  "x: "  + str(point_4d[0]) + "\n"
        myStr +=  "y: "  + str(point_4d[1]) + "\n"
        myStr +=  "z: "  + str(point_4d[2]) + "\n"


               
        y0, dy = 20, 25
        for i, line in enumerate(myStr.split('\n')):
            y = y0 + i*dy
            numpy_horizontal = cv2.putText(numpy_horizontal,line, (10,y),cv2.FONT_HERSHEY_SIMPLEX,0.6,(255,255,0),2,cv2.LINE_AA)
        
        # left image
        #define axes points in homogeneous coorindates
        axes = np.array([[0,0,0,1], [1,0,0,1], [0,1,0,1], [0,0,1,1]])
        #project to camera view
        projected_axes = (camera_matrix_left @ axes.T).T
        #Remove the homogeneous coordinate information
        projected_axes = projected_axes[:,:2]/projected_axes[:,2:3]
        #convert to integers because image space is integer coorindates
        projected_axes = projected_axes.astype('int32')
        #define some colors for your axes
        cols = [(0,0,255), (0,255,0), (255,0,0)] #the axes are drawn with ['red', 'green', 'blue'] colors

        #draw the axes on the camera image
        for _ax, _col in zip(projected_axes[1:], cols):
            _o = projected_axes[0] #origin point
            cv2.line(numpy_horizontal, (_o[0], _o[1]), (_ax[0], _ax[1]), _col, 2)
        
        # right image
        projected_axes = (camera_matrix_right @ axes.T).T
        #Remove the homogeneous coordinate information
        projected_axes = projected_axes[:,:2]/projected_axes[:,2:3]
        #convert to integers because image space is integer coorindates
        projected_axes = projected_axes.astype('int32')
        #define some colors for your axes
        cols = [(0,0,255), (0,255,0), (255,0,0)] #the axes are drawn with ['red', 'green', 'blue'] colors

        #draw the axes on the camera image
        for _ax, _col in zip(projected_axes[1:], cols):
            _o = projected_axes[0] #origin point
            cv2.line(numpy_horizontal, (_o[0], _o[1]), (_ax[0], _ax[1]), _col, 2)

        numpy_horizontal_concat = np.concatenate((left_rect, right_rect), axis=1)
        cv2.imshow("image", numpy_horizontal)
        cv2.namedWindow("image")
        cv2.setMouseCallback("image", onMouse)
        # cv2.imshow('Numpy Horizontal Concat', numpy_horizontal_concat)


        if cv2.waitKey(30) >= 0 :
            break

    exit(0)

if __name__ == "__main__":
    main()