Skip to content

Instantly share code, notes, and snippets.

@stephanschulz
Created April 13, 2022 14:53
Show Gist options
  • Save stephanschulz/158fb66c8f7516e0f95bc10a846bdb3f to your computer and use it in GitHub Desktop.
Save stephanschulz/158fb66c8f7516e0f95bc10a846bdb3f to your computer and use it in GitHub Desktop.
using python + zed + opencv without sdk
'''
///////////////////////////////////////////////////////////////////////////
//
// 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)
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 = 1280
height = 720
def onMouse(event, x, y, flags, param):
global left_point
global right_point
if event == cv2.EVENT_LBUTTONDOWN:
# cv2.circle(lastImage, (x, y), 3, (255, 0, 0), -1)
left_point = (x,y)
# print("left_point", left_point)
else :
right_point = (x,left_point[1])
# print("right_point", right_point)
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 = 1280
image_size.height = 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)
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)
# 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))
# start_point = (20,20)
# end_point = (200,200)
numpy_horizontal = cv2.line(numpy_horizontal, left_point, right_point,(255,255,0),2)
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)
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)
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()
@stephanschulz
Copy link
Author

stephanschulz commented Apr 13, 2022

I am trying to triangulate the 3d xyz from two sets of 2d points from a calibrated stereo camera.

I could use any stereo or depth camera and use the provided depth maps / point clouds to extract the needed 3D data BUT I feel most point clouds are too noisy. I think, that's because some sort ofautomated stereo pixel pair finding is used, like StereoSgbmPar.

So, picking my pixels pairs manually I am hoping to get better results.

Since I already have a the Stereolabs Zed RGB stereo camera I thought I should take advantage of the existing calibration file. Here is the file that Stereolabs provide based on my camera's serial number. I believe it contains the intrinsic and extrinsic parameters needed. This would avoid me having to do the checker board style calibrations (I think).

But in my test the 3d xyz values do not seem to make sense when using the non-SDK version of Stereolabs code.
I am using cv2.triangulatePoints not DLT as mentioned here.

I am probably skipping a few important steps in the above code, but am unsure which.
In the above article they are doing the whole calibration from scratch, while I am using the calibration file the Stereolabs provides for my specific camera.

Here is a short video from the above python app, in which I show how via the mouse I select the pixels pairs.
The xyz text print out shows how z most of the time is around -65; which must be wrong.
Apr-13-2022 10-51-492

I first actually tried this in C++ with my openframeworks.cc based source code here.
I based the C++ code on the collection of these examples which all do not need the Zed SDK

Here a video of my C++ app. In this app the rectified camera images are completely shown, not like in the python app which cuts off the corners.
Apr-13-2022 18-18-572

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment