diff --git a/.gitignore b/.gitignore index ccd98812d9d..3ae9355f562 100644 --- a/.gitignore +++ b/.gitignore @@ -89,3 +89,4 @@ librealsense-log.txt *.cxx .vscode/* +*.mp4 diff --git a/wrappers/python/applications/README.md b/wrappers/python/applications/README.md new file mode 100644 index 00000000000..6ac4aa41e56 --- /dev/null +++ b/wrappers/python/applications/README.md @@ -0,0 +1,72 @@ +# Intel RealSense - Object Camera + +Objects are everywhere! + +This package supports object data extraction from the intel Realsense camera RGB or Depth streams in real time. + +The list of the supported objects, frameworks, platforms and applications are below. Click on each image to find out more. + +# Objects + +- Barcodes, QR Codes, Aruco Markers - well defined objects detected by using RGB camera data. + +Barcodes | QR Codes | Aruco Markers | +:------------: | :----------: | :-------------: | +[![Barcode](barcode/doc/barcode_camera.gif)](barcode/README.md) | [![QR Codes](barcode/doc/qrcode_camera.gif)](barcode/README.md) | [![Aruco](barcode/doc/aruco_camera.gif)](barcode/README.md) | + +- Planes, Edges, Corners - 3D geometric objects detected using depth camera information. + +Planes | Edges | Corners | +:------------: | :----------: | :-------------: | +[![Planes](planes/doc/plane_3.gif)](planes/README.md) | [![Edges](planes/doc/edges-1.gif)](planes/README.md) | [![Corners](planes/doc/corner-1.gif)](planes/README.md) | + + + + +# Utilities + +OpenCV Like Camera Wrapper - controls low level camera features like exposure, gain, laser power, provides access to left and right sensor images and additional features. +It can be used to connect to Real Sense camera in real time. It provides a simple UI to store videos and images in different formats. + +[![RGB](utils/doc/cam_rgb_d.jpg)](utils/README.md) | [![D16](utils/doc/cam_d_3.jpg)](utils/README.md) | [![SC1](utils/doc/cam_d_5.jpg)](utils/README.md) | +:------------: | :----------: | :-------------: | +[![IRL](utils/doc/cam_d_9.jpg)](utils/README.md) | [![EXPOSURE](utils/doc/cam_e_2.jpg)](utils/README.md) | [![GAIN](utils/doc/cam_g_5.jpg)](utils/README.md) | +[![PWR](utils/doc/cam_p_0.jpg)](utils/README.md) | [![SC2](utils/doc/image_sc2_000.png)](utils/README.md) | [![D16](utils/doc/image_d16_000.png)](utils/README.md) | + +Depth Noise measurement tool - a simple UI to measure noise in the depth data. It supports measurement of the temporal noise per pixel, spatial noise per image ROI and fitted plane noise. In addition, it reports the number/percent of the non valid pixels. The results are visualized and alo printed in the console window. + +Temporal/Spatial ROI | Plane Fit ROI | +:------------: | :----------: | +[![STP](utils/doc/noise_measure_stp.gif)](utils/README.md) | [![PFIT](utils/doc/noise_measure.gif)](utils/README.md) | + + +Logger - to monitor and print console information, errors and processing time of the applications. + +[![LOG](utils/doc/noise_error_log.jpg)](utils/README.md) + + +# Request Camera Feature +If you want to run the application or object detection on the camera hardware - check this [link](https://docs.google.com/forms/d/e/1FAIpQLSdduDbnrRExDGFQqWAn8pX7jSr8KnwBmwuFOR9dgUabEp0F1A/viewform). + +# Supported Platforms and Compute Environments + +The following is the check list of supported environments and functionality: +- Windows +- Ubuntu +- Jetson (NVIDIA) - TBD +- Raspeberry PI - TBD + +# How to Contribute + +We greatly appreciate contributions from the community, including examples, applications, and guides. If you'd like to contribute, please follow these guidelines: + +1. **Create a pull request (PR)** with the title prefix `[RS]`, adding your new example folder to the `root/` directory within the repository. + +2. **Ensure your project adheres to the following standards:** + - Makes use of the `vision` package. + - Includes a `README.md` with clear instructions for setting up and running the example. + - Avoids adding large files or dependencies unless they are absolutely necessary for the example. + + + + diff --git a/wrappers/python/applications/barcode/README.md b/wrappers/python/applications/barcode/README.md new file mode 100644 index 00000000000..5de656f7db8 --- /dev/null +++ b/wrappers/python/applications/barcode/README.md @@ -0,0 +1,79 @@ +![](doc/show_examples.jpg) + +[![image](https://img.shields.io/pypi/v/scikit-spatial.svg)](https://pypi.python.org/pypi/scikit-spatial) +[![image](https://anaconda.org/conda-forge/scikit-spatial/badges/version.svg)](https://anaconda.org/conda-forge/scikit-spatial) +[![image](https://img.shields.io/pypi/pyversions/scikit-spatial.svg)](https://pypi.python.org/pypi/scikit-spatial) + + +# Introduction + +This code provides examples of how to detect Barcodes, Aruco Markers and QR Codes using RealSense camera in real time. +The object detection is based on RGB values of the camera. The following objects are supported: + +- Barcodes, QR Codes : detected by using PyZbar external library +- Aruco Markers : using OpenCV + +These objects could be integrated in your robotics and video processing pipe line. +Please be aware that /qrcode/aruco detection relies on the libraries in use. +Please refer to library limitations if barcode, qrcode or aruco marker are not detected. + + +# Modules and License + +We are using pyzbar and opencv contributed to detect barcodes and aruco markers. + + +# Installation Windows + +1. Install python 3.10 from Python Release Python 3.10.0 | + +2. Create virtual environment. In Windows PowerShell: + + python -m venv "your path"\Envs\barcode + +3. Activate virtual environment. In Windows CMD shell: + + "your path"\Envs\barcode\Scripts\activate.bat + +4. Installing realsense python wrapper for librealsense SDK. For example, download pyrealsense2-2.55.10.6089-cp310-cp310-win_amd64.whl (the latest : ![link](https://github.com/IntelRealSense/librealsense/releases/download/v2.56.3/pyrealsense2-2.56.3.7838-cp310-cp310-win_amd64_beta.whl)): + + pip install pyrealsense2-2.55.10.6089-cp310-cp310-win_amd64.whl + +5. Install opencv and numpy: + + pip install opencv-contrib-python + +6. Instrall PyZbar library: + + pip install pyzbar + +7. Install scipy: + + pip install scipy + +8. Install matplotlib: + + pip install matplotlib + +# Usage + +```py +>>> +>>> python barcode_realsense_detector.py + +``` +or +```py +>>> +>>> python aruco_realsense_detector.py # run the example + +``` + +# Troubleshooting + +## During PyZbar installation if the following error happens: + +FileNotFoundError: Could not find module '\Envs\barcode\lib\site-packages\pyzbar\libzbar-64.dll' (or one of its dependencies). Try using the full path with constructor syntax. + +Fix: Install vcredist_x64.exe From +Download Visual C++ Redistributable Packages for Visual Studio 2013 from Official Microsoft Download Center \ No newline at end of file diff --git a/wrappers/python/applications/barcode/doc/aruco_camera.gif b/wrappers/python/applications/barcode/doc/aruco_camera.gif new file mode 100644 index 00000000000..56db3593e2d Binary files /dev/null and b/wrappers/python/applications/barcode/doc/aruco_camera.gif differ diff --git a/wrappers/python/applications/barcode/doc/barcode_camera.gif b/wrappers/python/applications/barcode/doc/barcode_camera.gif new file mode 100644 index 00000000000..b633a5e6aa3 Binary files /dev/null and b/wrappers/python/applications/barcode/doc/barcode_camera.gif differ diff --git a/wrappers/python/applications/barcode/doc/qr_code_1.png b/wrappers/python/applications/barcode/doc/qr_code_1.png new file mode 100644 index 00000000000..3f5ee65d093 Binary files /dev/null and b/wrappers/python/applications/barcode/doc/qr_code_1.png differ diff --git a/wrappers/python/applications/barcode/doc/qr_codes_2.jpg b/wrappers/python/applications/barcode/doc/qr_codes_2.jpg new file mode 100644 index 00000000000..a6319452b33 Binary files /dev/null and b/wrappers/python/applications/barcode/doc/qr_codes_2.jpg differ diff --git a/wrappers/python/applications/barcode/doc/qrcode_camera.gif b/wrappers/python/applications/barcode/doc/qrcode_camera.gif new file mode 100644 index 00000000000..a7357693eb1 Binary files /dev/null and b/wrappers/python/applications/barcode/doc/qrcode_camera.gif differ diff --git a/wrappers/python/applications/barcode/doc/show_examples.jpg b/wrappers/python/applications/barcode/doc/show_examples.jpg new file mode 100644 index 00000000000..63498ef0965 Binary files /dev/null and b/wrappers/python/applications/barcode/doc/show_examples.jpg differ diff --git a/wrappers/python/applications/barcode/src/aruco_realsense_detector.py b/wrappers/python/applications/barcode/src/aruco_realsense_detector.py new file mode 100644 index 00000000000..befc5239fac --- /dev/null +++ b/wrappers/python/applications/barcode/src/aruco_realsense_detector.py @@ -0,0 +1,425 @@ +# -*- coding: utf-8 -*- +#cython: language_level=3 +#distutils: language=c++ + +""" +Aruco Marker Detection + +----------------------------- + Ver Date Who Descr +----------------------------- +2106 05.07.24 UD Suppoprt old version for compile. Bug fix +1911 11.02.24 UD Aruco detector. +----------------------------- +""" + + +import cv2 +import numpy as np +import copy + + + +pattern_size = (9,6) +square_size = 10.5 + +ARUCO_DICT = { + "DICT_4X4_50": cv2.aruco.DICT_4X4_50, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, + "DICT_4X4_250": cv2.aruco.DICT_4X4_250, + "DICT_4X4_1000": cv2.aruco.DICT_4X4_1000, + "DICT_5X5_50": cv2.aruco.DICT_5X5_50, + "DICT_5X5_100": cv2.aruco.DICT_5X5_100, + "DICT_5X5_250": cv2.aruco.DICT_5X5_250, + "DICT_5X5_1000": cv2.aruco.DICT_5X5_1000, + "DICT_6X6_50": cv2.aruco.DICT_6X6_50, + "DICT_6X6_100": cv2.aruco.DICT_6X6_100, + "DICT_6X6_250": cv2.aruco.DICT_6X6_250, + "DICT_6X6_1000": cv2.aruco.DICT_6X6_1000, + "DICT_7X7_50": cv2.aruco.DICT_7X7_50, + "DICT_7X7_100": cv2.aruco.DICT_7X7_100, + "DICT_7X7_250": cv2.aruco.DICT_7X7_250, + "DICT_7X7_1000": cv2.aruco.DICT_7X7_1000, + "DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL, + "DICT_APRILTAG_16h5": cv2.aruco.DICT_APRILTAG_16h5, + "DICT_APRILTAG_25h9": cv2.aruco.DICT_APRILTAG_25h9, + "DICT_APRILTAG_36h10": cv2.aruco.DICT_APRILTAG_36h10, + "DICT_APRILTAG_36h11": cv2.aruco.DICT_APRILTAG_36h11 +} + +# Things to draw objects +def draw_axis(img, imgpts, objid): + + imgpts = imgpts.astype(np.int32) + fs, ft, ff, fl = 0.9, 2, cv2.FONT_HERSHEY_SIMPLEX, 10 + + origin = tuple(imgpts[0].ravel()) + img = cv2.arrowedLine(img, origin, tuple(imgpts[1].ravel()), (0,0,255), 5) + img = cv2.arrowedLine(img, origin, tuple(imgpts[2].ravel()), (0,255,0), 5) + img = cv2.arrowedLine(img, origin, tuple(imgpts[3].ravel()), (255,0,0), 5) + + + cv2.putText(img ,'X',tuple(fl + imgpts[1].ravel()), ff, fs,(0,0,255),ft,cv2.LINE_AA) + cv2.putText(img ,'Y',tuple(fl + imgpts[2].ravel()), ff, fs,(0,255,0),ft,cv2.LINE_AA) + cv2.putText(img ,'Z',tuple(fl + imgpts[3].ravel()), ff, fs,(255,0,0),ft,cv2.LINE_AA) + + cv2.putText(img ,str(int(objid)),tuple(fl + imgpts[0].ravel()), ff, fs,(255,255,255),ft,cv2.LINE_AA) + + return img + +#%% Main +class ObjectAruco: + """ + The object part that need to be detected and estimate its pose + """ + + def __init__(self): + + self.name = 'aruco' + + #self.estimate6d = objpose.ObjPose() + + self.pattern_points = [] + self.init_pattern(pattern_size, square_size) + + self.camera_matrix = np.array([[1000.0,0.0,640.0],[0,1000.0,360.0],[0,0,1.0]]) + self.dist_coeffs = np.zeros((1,5)) + self.marker_length = 100 # mm marker size + self.scale_factor = 1 # image scaling + + self.aruco_type = ARUCO_DICT["DICT_6X6_250"] + self.corners = [] # aruco corners array + self.ids = [] # aruco ids array + + self.debugOn = False + + + + def init(self, pose_cfg = None): + """ + initialized for detection + + """ + + global pattern_size + p_size = self.cfg.get('chess_board_size') + if p_size is not None: + pattern_size = p_size + + sq_size = np.array(self.cfg['square_size']) + self.init_pattern(square_size = sq_size) + + # check cfg is initialized + self.camera_matrix = np.array(self.cfg['CamMatrix']) + self.dist_coeffs = np.array(self.cfg['CamDist']) + + stype = self.cfg.get('aruco_type') + self.aruco_type = ARUCO_DICT["DICT_6X6_250"] if stype is None else ARUCO_DICT[str(stype)] + + slen = self.cfg.get('aruco_side_length') + self.marker_length = 10 if slen is None else np.array(slen) + + sfact = self.cfg.get('scale_factor') + self.scale_factor = 1 if sfact is None else np.array(sfact) + self.Print('Configured parameters : Aruco type %s and size %s. Scale Factor %s' %(str(self.aruco_type),str(self.marker_length),str(self.scale_factor))) + + + def detect_aruco_pose(self, rgb_image): + + """ + detect multiple aruco + + """ + + # select object to work with + objects = {'objectId': [], 'rvecAll': [], 'tvecAll': [], 'objectQ': []} + + corners, ids, rejected = self.detect_aruco(rgb_image) + self.corners = corners + self.ids = ids + aruco_num = len(corners) + if aruco_num < 1: + #self.Print("Failed to find marker" , 'W') + return objects + + # flatten the ArUco IDs list + #ids = ids.flatten() + # loop over the detected ArUCo corners + objectId, rvecAll, tvecAll, objectQ = [], [], [], [] + for (markerCorner, markerID) in zip(corners, ids.flatten()): + # extract the marker corners (which are always returned in + # top-left, top-right, bottom-right, and bottom-left order) + corners = markerCorner.reshape((4, 2)) + (topLeft, topRight, bottomRight, bottomLeft) = corners + # convert each of the (x, y)-coordinate pairs to integers + topRight = (int(topRight[0]), int(topRight[1])) + bottomRight = (int(bottomRight[0]), int(bottomRight[1])) + bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1])) + topLeft = (int(topLeft[0]), int(topLeft[1])) + + centerX = (topRight[0] + bottomRight[0] + bottomLeft[0] + topLeft[0]) / 4 + centerY = (topRight[1] + bottomRight[1] + bottomLeft[1] + topLeft[1]) / 4 + + # adjust things + mark_length = self.marker_length + mark_corners = markerCorner/self.scale_factor + + # marker length in meters + rvec, tvec, _points = cv2.aruco.estimatePoseSingleMarkers(mark_corners, mark_length, self.camera_matrix, self.dist_coeffs) + + # diagonal length have length more than 50 is ok + quality = np.minimum(1,((topRight[1] - topLeft[1])**2 + (topRight[0] - topLeft[0])**2) / 2500) + + objectId.append(markerID) + rvecAll.append(rvec.reshape((3,1))) + #tvecAll.append(np.array([centerX,centerY,0]).reshape((3,1))) + tvecAll.append(tvec.reshape((3,1))) + objectQ.append(quality) + + + + # save to output + objects['objectId'] = objectId + objects['rvecAll'] = rvecAll + objects['tvecAll'] = tvecAll + objects['objectQ'] = objectQ # reliable + + return objects + + + def detect_aruco(self, frame_rgb, objectsInfo = None): + # aruco detection according to the type + + img = frame_rgb + aruco_type = self.aruco_type + + # support old version of open cv + vers = cv2.__version__.split('.') + if int(vers[1]) < 6: + arucoDict = cv2.aruco.Dictionary_get(aruco_type) + arucoParams = cv2.aruco.DetectorParameters_create() + #(corners, ids, rejected) = cv2.aruco.detectMarkers(img, arucoDict, parameters=arucoParams) + + + #dictionary = cv2.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_250) + #arucoParams = cv2.aruco.DetectorParameters() + #detector = cv.aruco.ArucoDetector(dictionary, parameters) + else: + + arucoDict = cv2.aruco.getPredefinedDictionary(aruco_type) + arucoParams = cv2.aruco.DetectorParameters() + + (corners, ids, rejected) = cv2.aruco.detectMarkers(img, arucoDict, parameters=arucoParams) + + #self.Print('%d arucos detected' %len(corners)) + + return corners, ids, rejected + + + def init_pattern(self, pattern_size = (9,6), square_size = 10.5): + # chessboard pattern init + self.pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 ) + self.pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2) + self.pattern_points *= square_size + + def draw_markers(self, color_image): + # show image with markers previously detected + corners, ids = self.corners, self.ids + corners = corners/self.scale_factor + + #color_image = self.draw_aruco(color_image, 1, corners, ids) + color_image = cv2.aruco.drawDetectedMarkers(color_image, corners, ids) + + return color_image + + def draw_image(self, color_image, corners, ids): + # show image and barcode + + #color_image = self.draw_aruco(color_image, 1, corners, ids) + color_image = cv2.aruco.drawDetectedMarkers(color_image, corners, ids) + + #cv2.imshow('Aruco Image',color_image) + #cv2.waitKey(100) + + return color_image + + + def draw_aruco(self, image, scaleFactor = 1, corners = [], ids = []): + " draw rectangle and id on the image" + if len(corners) < 1: + return image + # flatten the ArUco IDs list + ids = ids.flatten() + # loop over the detected ArUCo corners + for (markerCorner, markerID) in zip(corners, ids): + # extract the marker corners (which are always returned in + # top-left, top-right, bottom-right, and bottom-left order) + corners = markerCorner.reshape((4, 2)) + (topLeft, topRight, bottomRight, bottomLeft) = corners + # convert each of the (x, y)-coordinate pairs to integers + topRight = (int(topRight[0]), int(topRight[1])) + bottomRight = (int(bottomRight[0]), int(bottomRight[1])) + bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1])) + topLeft = (int(topLeft[0]), int(topLeft[1])) + + # draw the bounding box of the ArUCo detection + cv2.line(image, topLeft, topRight, (0, 255, 0), 2) + cv2.line(image, topRight, bottomRight, (0, 255, 0), 2) + cv2.line(image, bottomRight, bottomLeft, (0, 255, 0), 2) + cv2.line(image, bottomLeft, topLeft, (0, 255, 0), 2) + + # compute and draw the center (x, y)-coordinates of the ArUco + cX = int((topLeft[0] + bottomRight[0]) / 2.0) + cY = int((topLeft[1] + bottomRight[1]) / 2.0) + cv2.circle(image, (cX, cY), 4, (0, 0, 255), -1) + + # draw the ArUco marker ID on the image + cv2.putText(image, str(markerID), (topLeft[0], topLeft[1] - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + print("[INFO] ArUco marker ID: {}".format(markerID)) + + # show the output image +# cv2.imshow("Image", image) +# cv2.waitKey(0) + + # support resize + return image + + def draw_axis_all(self, img, rvecAll, tvecAll): + "show axis of the aruco" + cube_size = 50 + axis_te = np.float32([[0,0,0],[cube_size,0,0], [0,cube_size,0], [0,0,cube_size]]).reshape(-1,3) + ny, nx, nz = np.shape(img) + num_to_show = len(rvecAll) + mtrx = self.camera_matrix + dist = self.dist_coeffs + + # select object to work with + for ii in range(num_to_show): + + rvecCO = rvecAll[ii] + tvecCO = tvecAll[ii] + #objQ = objectQ[ii] + objId = ii+1 + + # show + imgpts_axis, jac = cv2.projectPoints(axis_te, rvecCO, tvecCO, mtrx, dist) + if ((np.fabs(imgpts_axis) > ny+nx).sum())== 0: + + # object coordinates + img = draw_axis(img, imgpts_axis, objId) + + + return img + + def get_object_pose(self, object_points, image_points, camera_matrix, dist_coeffs): + #ret, rvec, tvec = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs) + # similarity to object + ret, rvec, tvec, inliners = cv2.solvePnPRansac(object_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_EPNP) + + return rvec.flatten(), tvec.flatten() + + + def Print(self, txt='',level='I'): + + if level == 'I': + ptxt = 'I: ARU: %s' % txt + #logging.info(ptxt) + if level == 'W': + ptxt = 'W: ARU: %s' % txt + #logging.warning(ptxt) + if level == 'E': + ptxt = 'E: ARU: %s' % txt + #logging.error(ptxt) + print(ptxt) + + + +# ----------------------------------------- +def test_image(): + import glob + barcode_list = [] + img_list = glob.glob(r'..videos\*.jpg') + objAruco = ObjectAruco() + + namew = ' Aruco Demo : q - to quit' + cv2.namedWindow(namew,cv2.WINDOW_AUTOSIZE) + + for i, img_name in enumerate(img_list): + img = cv2.imread(img_name) + img = cv2.resize(img, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA) + + objects = objAruco.detect_aruco_pose(img) + img = objAruco.draw_image(img, objAruco.corners, objAruco.ids) + + cv2.imshow(namew, img) + ch = cv2.waitKey() & 0xFF + if ch == ord('q'): + break + +# ------------------------------------------- +def test_camera(): + cap = cv2.VideoCapture(0) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + + objAruco = ObjectAruco() + + namew = ' Aruco Demo : q - to quit' + cv2.namedWindow(namew,cv2.WINDOW_AUTOSIZE) + + while(cap.isOpened()): + + ret, img = cap.read() + if not ret: + print('The End') + break + + objects = objAruco.detect_aruco_pose(img) + img = objAruco.draw_image(img, objAruco.corners, objAruco.ids) + img = objAruco.draw_axis_all(img, objects['rvecAll'], objects['tvecAll'] ) + + + cv2.imshow(namew, img) + ch = cv2.waitKey(10) & 0xFF + if ch == ord('q'): + break + +# ---------------------------------------- +def test_video(): + + fname = r'file:///D:/Users/zion/Downloads/fidatu_emulation.avi' + cap = cv2.VideoCapture(fname) + objAruco = ObjectAruco() + + namew = ' Aruco Demo : q - to quit' + cv2.namedWindow(namew,cv2.WINDOW_AUTOSIZE) + + while(cap.isOpened()): + + ret, img = cap.read() + if not ret: + print('The End') + break + + objects = objAruco.detect_aruco_pose(img) + img = objAruco.draw_image(img, objAruco.corners, objAruco.ids) + + cv2.imshow(namew,img) + ch = cv2.waitKey(10) & 0xFF + if ch == ord('q'): + break + + cv2.destroyAllWindows() + print('Video done') + +# -------------------------- +if __name__ == '__main__': + print(__doc__) + + #test_image() + test_camera() # ok + #test_video() + + + + \ No newline at end of file diff --git a/wrappers/python/applications/barcode/src/barcode_realsense_detector.py b/wrappers/python/applications/barcode/src/barcode_realsense_detector.py new file mode 100644 index 00000000000..b3d8dd9760d --- /dev/null +++ b/wrappers/python/applications/barcode/src/barcode_realsense_detector.py @@ -0,0 +1,417 @@ + + +""" +Created on Sep 04 16:53:13 2019 +Debug module that simulates detection + +----------------------------- + Ver Date Who Descr +----------------------------- +0101 01.07.24 UD Created +----------------------------- +""" + +import cv2 +import numpy as np +import copy +try: + from pyzbar.pyzbar import decode , ZBarSymbol +except: + print('Warning : Barcode lib is not found on not installed.') + decode , ZBarSymbol = None, None + +# ---------------------------------------- +class ObjectAbstract: + """ + standard abstract type for the RealSense object + mostly defines output and data representation + """ + def __init__(self): + + self.name = 'general' + self.id = 1 # many different object + self.pose = np.zeros((6,1)) # Tx,Ty,Tz in mm and Rx, Ry Rz in degrees + self.quality = 0 # quality between 0-1 + + def get_object_data(self): + "read object data" + object_data = {'name':self.name, 'id':self.id, 'pose':self.pose,'quality':self.quality} + return object_data + + + +# ---------------------------------------- +class ObjectBarcode(ObjectAbstract): + + """ + Creates and manage barcode detector + + """ + + def __init__(self): + + self.name = 'barcode' + self.code = [] # barcode array + self.no_library = decode is None + self.debugOn = False + + + def detect_objects_old(self, rgb_image, objects = None): + + """ + detect multiple barcodes - select one which is in the middle of the page + + """ + + # select object to work with + objects = {'objectId': [], 'rvecAll': [], 'tvecAll': [], 'objectQ': [], 'objectText': []} + if self.no_library: + self.Print("Failed to find barcode library",'E' ) + return objects + + img_x_center = rgb_image.shape[1]/2 + + code, rgb_image = self.detect_barcode(rgb_image) + self.code = code + barcode_num = len(code) + if barcode_num < 1: + #self.Print("Failed to find barcode" , 'W') + return objects + + # compute positions in x of the barcodes + x_coord = [code[k].rect[0] for k in range(barcode_num)] + x_coord = np.array(x_coord) + + cent_ind = np.argmin(np.abs(x_coord - img_x_center)) + + # select the central only + barcode_txt = code[cent_ind].data.decode ('utf-8') + + # compute pose in x and y + (x, y, w, h) = code[cent_ind].rect + + # save to output + objects['objectId'] = [barcode_txt] + objects['rvecAll'] = [np.zeros((3,1))] + objects['tvecAll'] = [np.array([x,y,0]).reshape((3,1))] + objects['objectQ'] = [1] # reliable + objects['objectText'] = ['barcode'] # reliable + + return objects + + def detect_objects(self, rgb_image): + """ + detect multiple barcodes + + """ + + # select object to work with + objects = {'objectId': [], 'rvecAll': [], 'tvecAll': [], 'objectQ': [], 'objectText': []} + if self.no_library: + self.Print("Failed to find barcode library",'E' ) + return objects + + code, rgb_image = self.detect_barcode(rgb_image) + self.code = code + if len(code)<1: + #self.Print("Failed to find barcode" , 'W') + return objects + + barcode_txt = code[0].data.decode ('utf-8') + + # compute pose in x and y + (x, y, w, h) = code[0].rect + + + #rvec, tvec = self.get_object_pose(self.pattern_points, corners, self.camera_matrix, self.dist_coeffs) + + objects['objectId'] = [barcode_txt] + objects['rvecAll'] = [np.zeros((3,1))] + objects['tvecAll'] = [np.array([x,y,0]).reshape((3,1))] + objects['objectQ'] = [1] # reliable + objects['objectText'] = ['barcode'] # reliable + + return objects + + + + def detect_barcode(self, frame_rgb, objectsInfo = None): + # if detcted the region + + # use detcetion from nnet to extract the sub region + self.region_start_x = 0 + self.region_start_y = 0 + if objectsInfo is not None: + if len(objectsInfo['objectPoints']) > 0: + # xy array + object_points = objectsInfo['objectPoints'][0] + min_vals = object_points.min(axis = 0) + max_vals = object_points.max(axis = 0) + min_x = np.maximum(0, min_vals[0] - 5).astype(int) + max_x = np.minimum(frame_rgb.shape[1], max_vals[0] + 5).astype(int) + min_y = np.maximum(0, min_vals[1] - 15).astype(int) + max_y = np.minimum(frame_rgb.shape[0], max_vals[1] + 15).astype(int) + frame_rgb = frame_rgb[min_y:max_y,min_x:max_x,:] + + self.region_start_x = min_x + self.region_start_y = min_y + + + img = frame_rgb + + # read the image in numpy array using cv2 + #img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + #blur = cv2.GaussianBlur(img, (5, 5), 0) + #ret, bw_im = cv2.threshold(blur, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU) + #cv2.imshow('Barcode',img) + #cv2.waitKey(10) + + #tstart = time.time() + rotation_angles_degree = [0] + for k in range(len(rotation_angles_degree)): + #rstart = time.time() + img_rot = self.rotate_image(img, rotation_angles_degree[k]) + #self.Print('Rotation time : %4.3f' %(time.time() - rstart)) + code = decode(img_rot) #, symbols=[ZBarSymbol.CODE128]) + #self.Print('Detection time : %4.3f' %(time.time() - rstart)) + if len(code) > 0: + break + #self.Print('Total Detection time : %4.3f' %(time.time() - tstart)) + # debug + #print (code) + #self.draw_image(img_rot, code) + + return code, img + + def draw_image(self, color_image, barcode = []): + # show image and barcode + color_image = self.draw_barcode(color_image, 1, barcode) + cv2.imshow('Barcode Image',color_image) + cv2.waitKey(100) + + return color_image + + def rotate_image(self, img, rot_ang): + # rotate image by degrees to simplify detection + h, w = img.shape[0], img.shape[1] + degree = rot_ang + R = cv2.getRotationMatrix2D((w / 2, h / 2), degree, 1) + img_rot = copy.deepcopy(img) + img_rot = cv2.warpAffine(img_rot, R, (w, h), flags=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT, borderValue=0) + + return img_rot + + def draw_barcode(self, color_image, scaleFactor = 1, detectedBarcodes = None): + + detectedBarcodes = self.code if detectedBarcodes is None else detectedBarcodes + + # Traverse through all the detected barcodes in image + for barcode in detectedBarcodes: + + # Locate the barcode position in image + (x, y, w, h) = barcode.rect + # correct for subregion + x, y, w, h = int(x * scaleFactor), int(y * scaleFactor), int(w * scaleFactor), int(h * scaleFactor) + + # Put the rectangle in image using + # cv2 to highlight the barcode + cv2.rectangle(color_image, (x-10, y-10), (x + w+10, y + h+10), (0, 255, 255), 2) + cv2.putText(color_image, (barcode.data.decode ('utf-8')), (x-100, y-20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA, False) + + if barcode.data !="" and self.debugOn: + + # Print the barcode data + print(barcode.data.decode ('utf-8')) + print(barcode.type) + print(barcode.rect) + + # support resize + return color_image + + def Print(self, txt='',level='I'): + + if level == 'I': + ptxt = 'I: BD: %s' % txt + #logging.info(ptxt) + if level == 'W': + ptxt = 'W: BD: %s' % txt + #logging.warning(ptxt) + if level == 'E': + ptxt = 'E: BD: %s' % txt + #logging.error(ptxt) + + print(ptxt) + +# ---------------------------------------- +# Tests +# ---------------------------------------- + +def test_image(): + "read barcodes from images" + import glob + barcode_list = [] + img_list = glob.glob(r'..\data\*.jpg') + oBarCode = ObjectBarcode() + + for i, img_name in enumerate(img_list): + img = cv2.imread(img_name) + objects = oBarCode.pose6d_detect_objects(img) +# code = +# barcode_list.append(objects) +# if len(code)<1: +# raise Exception("Failed to find corners in img # %d" % i) + img = oBarCode.draw_barcode(img ) + oBarCode.draw_image(img) + + cv2.imshow('bars',img) + ch = cv2.waitKey() & 0xFF + if ch == ord('q'): + break + +def test_camera(): + cap = cv2.VideoCapture(0) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + + oBarCode = ObjectBarcode() + + namew = 'Barcode Demo : q - to quit' + cv2.namedWindow(namew,cv2.WINDOW_AUTOSIZE) + + while(cap.isOpened()): + + ret, img = cap.read() + if not ret: + print('The End') + break + + objects = oBarCode.pose6d_detect_objects(img) + img = oBarCode.draw_barcode(img ) + #oBarCode.draw_image(img) + + cv2.imshow(namew,img) + ch = cv2.waitKey(10) & 0xFF + if ch == ord('q'): + break + +def test_video(): + + fname = r'file:///D:/Users/zion/Downloads/fixture_aruco_test (1).avi' + cap = cv2.VideoCapture(fname) + oBarCode = ObjectBarcode() + + namew = 'Barcode Demo : q - to quit' + cv2.namedWindow(namew,cv2.WINDOW_AUTOSIZE) + + while(cap.isOpened()): + + ret, img = cap.read() + if not ret: + print('The End') + break + + objects = oBarCode.pose6d_detect_objects(img) + img = oBarCode.draw_barcode(img ) + #oBarCode.draw_image(img) + + cv2.imshow(namew,img) + ch = cv2.waitKey(10) & 0xFF + if ch == ord('q'): + break + + cv2.destroyAllWindows() + print('Video done') + +def test_camera_realsense(): + """ + Real Sense camera connect + """ + import pyrealsense2 as rs + + # Configure depth and color streams + pipeline = rs.pipeline() + config = rs.config() + + # Get device product line for setting a supporting resolution + pipeline_wrapper = rs.pipeline_wrapper(pipeline) + pipeline_profile = config.resolve(pipeline_wrapper) + device = pipeline_profile.get_device() + device_product_line = str(device.get_info(rs.camera_info.product_line)) + + found_rgb = False + for s in device.sensors: + if s.get_info(rs.camera_info.name) == 'RGB Camera': + found_rgb = True + break + if not found_rgb: + print("The demo requires Depth camera with Color sensor") + exit(0) + + #config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) + config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) + + # Start streaming + pipeline.start(config) + + # barcode + oBarCode = ObjectBarcode() + + namew = 'RealSense Barcode Demo : q - to quit' + cv2.namedWindow(namew,cv2.WINDOW_AUTOSIZE) + + try: + while True: + + # Wait for a coherent pair of frames: depth and color + frames = pipeline.wait_for_frames() + depth_frame = True #frames.get_depth_frame() + color_frame = frames.get_color_frame() + if not depth_frame or not color_frame: + continue + + # Convert images to numpy arrays + #depth_image = np.asanyarray(depth_frame.get_data()) + color_image = np.asanyarray(color_frame.get_data()) + + # Apply colormap on depth image (image must be converted to 8-bit per pixel first) + #depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) + + #depth_colormap_dim = depth_colormap.shape + color_colormap_dim = color_image.shape + + # If depth and color resolutions are different, resize color image to match depth image for display + # if depth_colormap_dim != color_colormap_dim: + # resized_color_image = cv2.resize(color_image, dsize=(depth_colormap_dim[1], depth_colormap_dim[0]), interpolation=cv2.INTER_AREA) + # images = np.hstack((resized_color_image, depth_colormap)) + # else: + # images = np.hstack((color_image, depth_colormap)) + images = color_image + + # Show images + #cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) + #cv2.imshow('RealSense', images) + #cv2.waitKey(1) + + objects = oBarCode.detect_objects(images) + img = oBarCode.draw_barcode(images) + + + cv2.imshow(namew,img) + ch = cv2.waitKey(10) & 0xFF + if ch == ord('q'): + break + + finally: + + # Stop streaming + pipeline.stop() + + +# -------------------------- +if __name__ == '__main__': + print(__doc__) + + #test_camera() # ok + #test_video() + #test_image() + test_camera_realsense() diff --git a/wrappers/python/applications/planes/README.md b/wrappers/python/applications/planes/README.md new file mode 100644 index 00000000000..022843c55ba --- /dev/null +++ b/wrappers/python/applications/planes/README.md @@ -0,0 +1,61 @@ +![](doc/planes.png) + +[![image](https://img.shields.io/pypi/v/scikit-spatial.svg)](https://pypi.python.org/pypi/scikit-spatial) + + +# Introduction + +This code provides examples of the using RealSense camera for detection of RGB and Depth objects. +The following objects are supported: + +- Planes : detected multiple planes in the depth image +- Edges : detecting edges / intersection of planes +- Corners: three plane intersection/ junctions + +These objects could be integrated in your robotics and video processing pipe line. + +Plane | Multi Plane | Fast Plane | +:------------: | :----------: | :-------------: | +[![Planes](doc/plane_2.gif)](planes/README.md) | [![Edges](doc/plane_3.gif)](planes/README.md) | [![Corners](doc/plane_4.gif)](planes/README.md) | + + + +## Installation Windows + +1. Install python 3.10 from Python Release Python 3.10.0 | + +2. Create virtual environment. In Windows PowerShell: + + python -m venv "your path"\Envs\planes + +3. Activate virtual environment. In Windows CMD shell: + + "your path"\Envs\planes\Scripts\activate.bat + +4. Installing realsense driver. For example, download pyrealsense2-2.55.10.6089-cp310-cp310-win_amd64.whl: + + pip install pyrealsense2-2.55.10.6089-cp310-cp310-win_amd64.whl + +5. Install opencv and numpy: + + pip install opencv-contrib-python + + + +## Usage + +Importing the code: + +```py +>>> from plane_detector import PlaneDetector + +>>> pd = PlaneDetector() + +``` + +Running tests: + +```py +>>> python test_plane_detector.py + +``` \ No newline at end of file diff --git a/wrappers/python/applications/planes/data/image_ddd_000.png b/wrappers/python/applications/planes/data/image_ddd_000.png new file mode 100644 index 00000000000..f01a2370a28 Binary files /dev/null and b/wrappers/python/applications/planes/data/image_ddd_000.png differ diff --git a/wrappers/python/applications/planes/doc/corner-1.gif b/wrappers/python/applications/planes/doc/corner-1.gif new file mode 100644 index 00000000000..7079b60b560 Binary files /dev/null and b/wrappers/python/applications/planes/doc/corner-1.gif differ diff --git a/wrappers/python/applications/planes/doc/edges-1.gif b/wrappers/python/applications/planes/doc/edges-1.gif new file mode 100644 index 00000000000..c562da17f4e Binary files /dev/null and b/wrappers/python/applications/planes/doc/edges-1.gif differ diff --git a/wrappers/python/applications/planes/doc/plane_2.gif b/wrappers/python/applications/planes/doc/plane_2.gif new file mode 100644 index 00000000000..db840c3b163 Binary files /dev/null and b/wrappers/python/applications/planes/doc/plane_2.gif differ diff --git a/wrappers/python/applications/planes/doc/plane_3.gif b/wrappers/python/applications/planes/doc/plane_3.gif new file mode 100644 index 00000000000..29333d30e10 Binary files /dev/null and b/wrappers/python/applications/planes/doc/plane_3.gif differ diff --git a/wrappers/python/applications/planes/doc/plane_4.gif b/wrappers/python/applications/planes/doc/plane_4.gif new file mode 100644 index 00000000000..e23985b052c Binary files /dev/null and b/wrappers/python/applications/planes/doc/plane_4.gif differ diff --git a/wrappers/python/applications/planes/doc/planes.png b/wrappers/python/applications/planes/doc/planes.png new file mode 100644 index 00000000000..8e68cca2d19 Binary files /dev/null and b/wrappers/python/applications/planes/doc/planes.png differ diff --git a/wrappers/python/applications/planes/src/plane_detector.py b/wrappers/python/applications/planes/src/plane_detector.py new file mode 100644 index 00000000000..9311e52be6b --- /dev/null +++ b/wrappers/python/applications/planes/src/plane_detector.py @@ -0,0 +1,828 @@ +#!/usr/bin/env python + +''' +Multi planar plain detector +================== + +Using depth image to compute depth planes locally for specific ROI. + + +Usage: + +Environemt : + ..\\Envs\\barcode + +Install : + + + +''' +import sys +import numpy as np +import cv2 as cv +import random + + + # importing common Use modules +sys.path.append(r'wrappers\python\applications\utils\src') +from logger import log + + +#%% Main +class PlaneDetector: + def __init__(self, detect_type = 'p'): + + self.detect_type = detect_type # plane + + self.frame_size = (1280,720) + self.img = None + self.cam_matrix = np.array([[1000,0,self.frame_size[0]/2],[0,1000,self.frame_size[1]/2],[0,0,1]], dtype = np.float32) + self.cam_distort = np.array([0,0,0,0,0],dtype = np.float32) + + self.img3d = None # contains x,y and depth plains + self.img_xyz = None # comntains X,Y,Z information after depth image to XYZ transform + self.img_mask = None # which pixels belongs to the plain + self.rect = None # roi + + # detector type + self.matrix_inv = None # holds inverse params of the + self.matrix_dir = None # direct u,v,1 + self.matrix_xyz = None # direct u,v,1 multiplied by z + self.matrix_index = None # index of the points in the original image + self.plane_params = None # rvec not normalized + self.plane_center = None # tvec + #self.corner_ind = [0, 10, 40, 50] # corner of the rectnagle for the projection + self.rect_3d = None # roi but projected on 3D + + # params + self.MIN_SPLIT_SIZE = 32 + self.MIN_STD_ERROR = 0.01 + + # color for the mask + self.color_mask = (0,255,0) # green + + # help variable + self.ang_vec = np.zeros((3,1)) # help variable + + def init_image(self, img = None): + "load image" + + + self.img = img + h,w = img.shape[:2] + self.frame_size = (w,h) + self.img_mask = np.zeros((h,w)) + return True + + def init_roi(self, test_type = 1): + "load the test case" + roi = [0,0,self.frame_size[0],self.frame_size[1]] + if test_type == 1: + roi = [310,230,330,250] # xlu, ylu, xrb, yrb + elif test_type == 2: + roi = [300,220,340,260] # xlu, ylu, xrb, yrb + elif test_type == 3: + roi = [280,200,360,280] # xlu, ylu, xrb, yrb + elif test_type == 4: + roi = [220,140,420,340] # xlu, ylu, xrb, yrb + elif test_type == 4: + roi = [200,120,440,360] # xlu, ylu, xrb, yrb + return roi + + def preprocess(self, img = None): + "image preprocessing - extracts roi and converts from uint8 to float using log function" + if img is None: + log.info('No image provided') + return False + + if self.img_mask is None: + ret = self.init_image(img) + + if self.rect is None: # use entire image + self.rect = self.init_roi(4) + + # init params of the inverse + if self.matrix_dir is None: + self.fit_plane_init_old() + + x0, y0, x1, y1 = self.rect + if len(img.shape) > 2: + img_roi = img[y0:y1,x0:x1,2].astype(np.float32) + else: + img_roi = img[y0:y1,x0:x1].astype(np.float32) + return img_roi + + def init_img3d(self, img = None): + "initializes xyz coordinates for each point" + img = self.img if img is None else img + h,w = img.shape[:2] + x = np.arange(w) + y = np.arange(h) + x,y = np.meshgrid(x,y) + fx = self.cam_matrix[0,0] + fy = self.cam_matrix[1,1] + + xy = np.hstack((x.reshape(-1,1),y.reshape(-1,1))) + xy = np.expand_dims(xy, axis=1).astype(np.float32) + xy_undistorted = cv.undistortPoints(xy, self.cam_matrix, self.cam_distort) + + u = xy_undistorted[:,0,0].reshape((h,w)) + v = xy_undistorted[:,0,1].reshape((h,w)) + z3d = img.astype(np.float32) + x3d = z3d.copy() + y3d = z3d.copy() + + #ii = np.logical_and(z3d> 1e-6 , np.isfinite(z3d)) + ii = z3d > 5 + x3d[ii] = u[ii]*z3d[ii] #/fx + y3d[ii] = v[ii]*z3d[ii] #/fy + z3d[ii] = z3d[ii] + + #self.img3d = np.stack((u/fx,v/fy,z3d), axis = 2) + self.img3d = np.stack((u,v,z3d), axis = 2) + self.img_mask = np.zeros((h,w)) + return self.img3d + + def compute_img3d(self, img = None): + "compute xyz coordinates for each point using prvious init" + img = self.img if img is None else img + xyz = self.img3d + if xyz is None: + xyz = self.init_img3d(img) + + if np.any(img.shape[:2] != xyz.shape[:2]): + print('Image dimension change') + return + + imgXYZ = self.img3d.copy() + + z3d = img.astype(np.float32) + x3d = self.img3d[:,:,0].copy() # u/f + y3d = self.img3d[:,:,1].copy() # v/f + + # filter bad z values + #ii = np.logical_and(z3d > 1e-6 , np.isfinite(z3d)) + ii = z3d > 15 + x3d[ii] = x3d[ii]*z3d[ii] + y3d[ii] = y3d[ii]*z3d[ii] + z3d[ii] = z3d[ii] + + # x,y,z coordinates in 3D + imgXYZ[:,:,0] = x3d + imgXYZ[:,:,1] = y3d + imgXYZ[:,:,2] = z3d + + self.img_xyz = imgXYZ + return imgXYZ + + def check_error(self, xyz1_mtrx, vnorm): + "checking the error norm" + err = np.dot(xyz1_mtrx, vnorm) + err_std = err.std() + return err_std + + def convert_plane_params(self, plane_equation): + "convert plane params to rvec" + # 4. Convert plane parameters to rvec and tvec + # - The plane normal vector is (A, B, C). + # - We can use the normal vector to get the rotation. + # - A point on the plane can be used for the translation vector. + + # Normalize the plane normal vector + normal = plane_equation #np.array([plane_equation[0], plane_equation[1], plane_equation[2]]) + normal_norm = np.linalg.norm(normal) + if normal_norm == 0: + log.error("Error: Zero norm for plane normal vector.") + return None + normal = normal / normal_norm + + # Use the normalized normal vector to get the rotation matrix + # This is a common method, but there are other ways to do this. + z_axis = np.array([0, 0, 1]) + rotation_axis = np.cross(z_axis, normal) + rotation_angle = np.arccos(np.dot(z_axis, normal)) + + # Handle the case where the rotation axis is zero (normal is parallel to z-axis) + if np.linalg.norm(rotation_axis) < 1e-6: + if normal[2] > 0: + rvec = np.zeros(3) # Rotation is identity + else: + rvec = np.array([0, np.pi, 0]) # Rotation by 180 degrees around X or Y. + else: + rvec, _ = cv.Rodrigues(rotation_axis * rotation_angle) + + return rvec + + def convert_plane_params_to_pose(self, plane_params = None, plane_center = None): + "converting params of the plane to the pose vector" + + plane_params = self.plane_params if plane_params is None else plane_params[:3].flatten() + plane_center = self.plane_center if plane_center is None else plane_center[:3].flatten() + + tvec = plane_center.reshape((1,-1)) + rvec = plane_params.reshape((1,-1)) #reshape((-1,1)) + rvec = rvec/np.linalg.norm(rvec.flatten()) + + pose_norm = np.hstack((tvec, rvec)) + #log.info('roi to pose') + return pose_norm #.flatten() + + def fit_plane_init(self): + "prepares data for real time fit a*x+b*y+c = z" + self.cam_matrix = np.array([[650,0,self.frame_size[0]/2],[0,650,self.frame_size[1]/2],[0,0,1]], dtype = np.float32) + self.cam_distort = np.array([0,0,0,0,0],dtype = np.float32) + + x0,y0,x1,y1 = 0,0,self.frame_size[0],self.frame_size[1] #self.rect + h,w = y1-y0, x1-x0 + x_grid = np.arange(x0, x1, 1) + y_grid = np.arange(y0, y1, 1) + x, y = np.meshgrid(x_grid, y_grid) + + # remember corner indexes for reprojection [0 .... h*(w-1)) + # . . + # h ......h*w-1] + #self.corner_ind = [0, h, h*w-1, h*(w-1), 0] + #h2,w2 = h>>1, w>>1 + #self.rect_3d = [[-w,-h,0],[w,-h,0],[w,h,0],[-w,h,0],[-w,-h,0]] + + # camera coordinates + xy = np.hstack((x.reshape(-1,1),y.reshape(-1,1))) + xy = np.expand_dims(xy, axis=1).astype(np.float32) + xy_undistorted = cv.undistortPoints(xy, self.cam_matrix, self.cam_distort) + + u = xy_undistorted[:,0,0].reshape((h,w)).reshape(-1,1) + v = xy_undistorted[:,0,1].reshape((h,w)).reshape(-1,1) + + # check + #u, v = u*self.cam_matrix[0,0], v*self.cam_matrix[1,1] + + self.matrix_dir = np.hstack((u,v,u*0+1)) + #self.matrix_inv = np.linalg.pinv(self.matrix_dir) + + def fit_plane_init_old(self): + "prepares data for real time fit a*x+b*y+c = z" + self.cam_matrix = np.array([[650,0,self.frame_size[0]/2],[0,650,self.frame_size[1]/2],[0,0,1]], dtype = np.float32) + self.cam_distort = np.array([0,0,0,0,0],dtype = np.float32) + + x0,y0,x1,y1 = self.rect + h,w = y1-y0, x1-x0 + x_grid = np.arange(x0, x1, 1) + y_grid = np.arange(y0, y1, 1) + x, y = np.meshgrid(x_grid, y_grid) + + # remember corner indexes for reprojection [0 .... h*(w-1)) + # . . + # h ......h*w-1] + #self.corner_ind = [0, h, h*w-1, h*(w-1), 0] + h2,w2 = h>>1, w>>1 + self.rect_3d = [[-w,-h,0],[w,-h,0],[w,h,0],[-w,h,0],[-w,-h,0]] + + # camera coordinates + xy = np.hstack((x.reshape(-1,1),y.reshape(-1,1))) + xy = np.expand_dims(xy, axis=1).astype(np.float32) + xy_undistorted = cv.undistortPoints(xy, self.cam_matrix, self.cam_distort) + + u = xy_undistorted[:,0,0].reshape((h,w)).reshape(-1,1) + v = xy_undistorted[:,0,1].reshape((h,w)).reshape(-1,1) + + # check + #u, v = u*self.cam_matrix[0,0], v*self.cam_matrix[1,1] + + self.matrix_dir = np.hstack((u,v,u*0+1)) + self.matrix_inv = np.linalg.pinv(self.matrix_dir) + + def convert_roi_to_points(self, img, point_num = 30, step_size = 1, roi_rect = None): + "converting roi to pts in XYZ - Nx3 array. point_num - is the target point number" + + # init params of the inverse + if self.matrix_xyz is None: # do not use mtrix_dir - initialized before + self.fit_plane_init() + + # deal iwth different rect options + roi_rect = self.rect if roi_rect is None else roi_rect + x0, y0, x1, y1 = roi_rect + + # make rectangle + h,w = y1-y0, x1-x0 + self.rect_3d = [[-w,-h,0],[w,-h,0],[w,h,0],[-w,h,0],[-w,-h,0]] + + # extract roi - must be compatible with image dimensions + # n,m = img.shape[:2] + # img_roi_mask = np.zeros((n,m), dtype = np.bool_) + # img_roi_mask[y0:y1,x0:x1] = True + # valid_bool = img_roi_mask > 0 & img > 0 + + # check if roi is valid + x_grid = np.arange(x0, x1, 1) + y_grid = np.arange(y0, y1, 1) + x, y = np.meshgrid(x_grid, y_grid) + flat_indices = np.ravel_multi_index((y, x), img.shape[:2]).reshape((-1,1)) + + # valid under mask + #valid_bool = img.flat[flat_indices] > 0 + #ii = flat_indices[valid_bool] + img_roi = img[y0:y1,x0:x1].astype(np.float32).reshape((-1,1)) + valid_bool = img_roi > 0 # valid pixels in the roi + ii = np.flatnonzero(valid_bool) + + #valid_bool = valid_bool.flatten() + #log.info(f'Timing : 1') + + # rect to show + #h,w = y1-y0, x1-x0 + #self.rect_3d = [[-w,-h,0],[w,-h,0],[w,h,0],[-w,h,0],[-w,-h,0]] + + # all non valid + #ii = np.where(valid_bool)[0] + + valid_point_num = len(ii) + if valid_point_num < 5: + return None + step_size = np.maximum(step_size, np.int32(valid_point_num/point_num)) + ii = ii[::step_size] + + # plane params - using only valid + #z = img.flat[ii].reshape((-1,1)) + z = img_roi[ii].reshape((-1,1)) + jj = flat_indices[ii].flatten() + xyz_matrix = self.matrix_dir[jj,:] + xyz_matrix[:,:3] = xyz_matrix[:,:3]*z # keep 1 intact + + #self.plane_center = xyz_center.flatten() + self.matrix_xyz = xyz_matrix + self.matrix_index = jj + + return xyz_matrix + + + def convert_roi_to_points_old(self, img_roi, point_num = 30, step_size = 1): + "converting roi to pts in XYZ - Nx3 array. point_num - is the target point number" + # x1,y1 = self.img_xyz.shape[:2] + # roi_area = x1*y1 + + # # reduce size of the grid for speed + # if step_size < 1 and roi_area > 100: + # step_size = np.maximum(1,int(np.sqrt(roi_area)/10)) + + + # #roi3d = self.img_xyz[y0:y1:step_size,x0:x1:step_size,:] + # roi3d = self.img_xyz[::step_size,::step_size,:] + # x,y,z = roi3d[:,:,0].reshape((-1,1)), roi3d[:,:,1].reshape((-1,1)), roi3d[:,:,2].reshape((-1,1)) + # xyz_matrix = np.hstack((x,y,z)) + # + + # init params of the inverse + if self.matrix_dir is None: + self.fit_plane_init_old() + + # extract roi + + n,m = img_roi.shape[:2] + img_roi = img_roi.reshape((-1,1)) + valid_bool = img_roi > 0 + valid_bool = valid_bool.flatten() + #log.info(f'Timing : 1') + + # all non valid + ii = np.where(valid_bool)[0] + valid_point_num = len(ii) + if valid_point_num < 5: + return None + step_size = np.maximum(step_size, np.int32(valid_point_num/point_num)) + ii = ii[::step_size] + + # plane params - using only valid + z = img_roi[ii] + xyz_matrix = self.matrix_dir[ii,:] + xyz_matrix[:,:3] = xyz_matrix[:,:3]*z # keep 1 intact + + # update corners of the rect in 3d + #self.rect_3d = self.matrix_dir[self.corner_ind,:]*img_roi[self.corner_ind] + # rect to show + x0, y0, x1, y1 = self.rect + h,w = y1-y0, x1-x0 + self.rect_3d = [[-w,-h,0],[w,-h,0],[w,h,0],[-w,h,0],[-w,-h,0]] + # substract mean + #xyz_center = xyz_matrix[:,:3].mean(axis=0) + #xyz_matrix = xyz_matrix - xyz_center + #log.info(f'Timing : 2') + + # mtrx_dir = np.hstack((self.matrix_dir[valid_bool,0]*z,self.matrix_dir[valid_bool,1]*z,z*0+1)) + # mtrx_inv = np.linalg.pinv(mtrx_dir) + # #mtrx_inv = self.matrix_inv[:,valid_bool] + # plane_params = np.dot(mtrx_inv,z) + + # decimate to make it run faster reduce size of the grid for speed. 1000 pix - 30x30 - step 1, 10000 pix - step=3 + #roi_area = n*m + #step_size = int(np.sqrt(roi_area)/7) if roi_area > 1000 else 1 + + #self.plane_center = xyz_center.flatten() + self.matrix_xyz = xyz_matrix + + return xyz_matrix + + def fit_plane_svd(self, img_roi): + "estimates mean and std of the plane fit" + + # roi converted to points with step size on the grid + xyz_matrix = self.convert_roi_to_points(img_roi, point_num = 250, step_size = 1) + #xyz_matrix = self.convert_roi_to_points_old(img_roi, point_num = 1e4, step_size = 1) + + # substract mean + xyz_center = xyz_matrix[:,:3].mean(axis=0) + xyz_matrix = xyz_matrix - xyz_center + #log.info(f'Timing : 2') + + # mtrx_dir = np.hstack((self.matrix_dir[valid_bool,0]*z,self.matrix_dir[valid_bool,1]*z,z*0+1)) + # mtrx_inv = np.linalg.pinv(mtrx_dir) + # #mtrx_inv = self.matrix_inv[:,valid_bool] + # plane_params = np.dot(mtrx_inv,z) + + # decimate to make it run faster reduce size of the grid for speed. 1000 pix - 30x30 - step 1, 10000 pix - step=3 + #roi_area = n*m + #step_size = int(np.sqrt(roi_area)/7) if roi_area > 1000 else 1 + + # using svd to make the fit + U, S, Vh = np.linalg.svd(xyz_matrix, full_matrices=True) + ii = np.argmin(S) + vnorm = Vh[ii,:] + #log.info(f'Timing : 3') + + # keep orientation + plane_params = vnorm*np.sign(vnorm[2]) + + # estimate error + err = np.dot(xyz_matrix,plane_params) + #z_est = z + err + xyz_center[2] + + img_mean = xyz_center[2] #z_est.mean() + img_std = err.std() + self.plane_params = plane_params[:3].flatten() + self.plane_center = xyz_center.flatten() + + #log.info(f'Plane : {self.plane_params}, error {img_std:.3f}, step {step_size}') + + return img_mean, img_std + + def fit_plane_svd_old(self, img_roi): + "estimates mean and std of the plane fit" + # n,m = img_roi.shape[:2] + # img_roi = img_roi.reshape((-1,1)) + # valid_bool = img_roi > 0 + # valid_bool = valid_bool.flatten() + # #log.info(f'Timing : 1') + + # # init params of the inverse + # if self.matrix_inv is None: + # self.fit_plane_init() + + # # plane params - using only valid + # z = img_roi[valid_bool] + # xyz_matrix = self.matrix_dir[valid_bool,:] + # xyz_matrix[:,:3] = xyz_matrix[:,:3]*z # keep 1 intact + + # update corners of the rect in 3d + #self.rect_3d = self.matrix_dir[self.corner_ind,:]*img_roi[self.corner_ind] + + # roi converted to points with step size on the grid + #xyz_matrix = self.convert_roi_to_points(img_roi, point_num = 1e4, step_size = 1) + xyz_matrix = self.convert_roi_to_points_old(img_roi, point_num = 1e4, step_size = 1) + + # substract mean + xyz_center = xyz_matrix[:,:3].mean(axis=0) + xyz_matrix = xyz_matrix - xyz_center + #log.info(f'Timing : 2') + + # mtrx_dir = np.hstack((self.matrix_dir[valid_bool,0]*z,self.matrix_dir[valid_bool,1]*z,z*0+1)) + # mtrx_inv = np.linalg.pinv(mtrx_dir) + # #mtrx_inv = self.matrix_inv[:,valid_bool] + # plane_params = np.dot(mtrx_inv,z) + + # decimate to make it run faster reduce size of the grid for speed. 1000 pix - 30x30 - step 1, 10000 pix - step=3 + #roi_area = n*m + #step_size = int(np.sqrt(roi_area)/7) if roi_area > 1000 else 1 + + # using svd to make the fit + U, S, Vh = np.linalg.svd(xyz_matrix, full_matrices=True) + ii = np.argmin(S) + vnorm = Vh[ii,:] + #log.info(f'Timing : 3') + + # keep orientation + plane_params = vnorm*np.sign(vnorm[2]) + + # estimate error + err = np.dot(xyz_matrix,plane_params) + #z_est = z + err + xyz_center[2] + + img_mean = xyz_center[2] #z_est.mean() + img_std = err.std() + self.plane_params = plane_params[:3].flatten() + self.plane_center = xyz_center.flatten() + + #log.info(f'Plane : {self.plane_params}, error {img_std:.3f}, step {step_size}') + + return img_mean, img_std + + def fit_plane_with_outliers(self, img_roi): + "computes normal for the specifric roi and evaluates error. Do it twice to reject outliers" + + # roi converted to points with step size on the grid + xyz_matrix = self.convert_roi_to_points_old(img_roi, point_num = 250, step_size = 0) + + # substract mean + xyz_center = xyz_matrix[:,:3].mean(axis=0) + xyz_matrix = xyz_matrix - xyz_center + + # using svd to make the fit to a sub group + U, S, Vh = np.linalg.svd(xyz_matrix, full_matrices=True) + ii = np.argmin(S) + vnorm = Vh[ii,:] + #vnorm = vnorm*np.sign(vnorm[2]) # keep orientation + + # keep orientation + plane_params = vnorm*np.sign(vnorm[2]) + + # estimate error + err = np.dot(xyz_matrix,plane_params) + err_std = err.std() + log.info('Fit error iteration 1: %s' %str(err_std)) + + # filter only the matching points + inlier_ind = np.abs(err) < 5*err_std + + # perform svd one more time + tvec = xyz_matrix[inlier_ind,:3].mean(axis=0)# + xyz1 = xyz_matrix[inlier_ind,:] - tvec + U, S, Vh = np.linalg.svd(xyz1, full_matrices=True) + ii = np.argmin(S) + vnorm = Vh[ii,:] + # keep orientation + plane_params = vnorm*np.sign(vnorm[2]) + + # checking error + err = np.dot(xyz1, plane_params) + err_std = err.std() + log.info('Fit error iteration 2: %s' %str(err_std)) + + # # We can convert this flat index to row and column indices + # row_index, col_index = np.unravel_index(inlier_ind, self.img_mask.shape) + # self.img_mask[row_index, col_index] = 1 + + img_mean = tvec[2] #z_est.mean() + img_std = err_std + self.plane_params = plane_params[:3].flatten() + self.plane_center = tvec.flatten() + + #log.info(f'Plane : {self.plane_params}, error {img_std:.3f}, step {step_size}') + + return img_mean, img_std + + def fit_plane_ransac(self, img_roi): + + """ + Find the best equation for a plane. + + :param pts: 3D point cloud as a `np.array (N,3)`. + :param thresh: Threshold distance from the plane which is considered inlier. + :param maxIteration: Number of maximum iteration which RANSAC will loop over. + :returns: + - `self.equation`: Parameters of the plane using Ax+By+Cy+D `np.array (1, 4)` + - `self.inliers`: points from the dataset considered inliers + + """ + #log.info('Fit ransac: ...') + # roi converted to points with step size on the grid + xyz_matrix = self.convert_roi_to_points_old(img_roi, point_num = 250, step_size = 1) + #xyz_matrix = self.convert_roi_to_points(img_roi, point_num = 250, step_size = 1) + if xyz_matrix is None: + log.error('No points in the ROI') + return 0, 0 + + thresh = 1.05 + maxIteration = 100 + + + n_points = xyz_matrix.shape[0] + best_eq = [] + best_inliers = [] + + for it in range(maxIteration): + + # Samples 3 random points + id_samples = random.sample(range(0, n_points), 3) + pt_samples = xyz_matrix[id_samples,:] + + # We have to find the plane equation described by those 3 points + # We find first 2 vectors that are part of this plane + # A = pt2 - pt1 + # B = pt3 - pt1 + + vecA = pt_samples[1, :] - pt_samples[0, :] + vecB = pt_samples[2, :] - pt_samples[0, :] + + # Now we compute the cross product of vecA and vecB to get vecC which is normal to the plane + vecC = np.cross(vecA, vecB) + vecC_norm = np.linalg.norm(vecC) + + # protect from the close spaced points + if vecC_norm < 10e-6: + continue + + # make sure that Z direction is positive + vecC = vecC * np.sign(vecC[2]) + + # The plane equation will be vecC[0]*x + vecC[1]*y + vecC[0]*z = -k + # We have to use a point to find k + vecC = vecC / vecC_norm + #k = -np.sum(np.multiply(vecC, pt_samples[1, :])) + k = -np.dot(vecC, pt_samples[1, :]) + plane_eq = [vecC[0], vecC[1], vecC[2], k] + + # Distance from a point to a plane + # https://mathworld.wolfram.com/Point-PlaneDistance.html + # pt_id_inliers = [] # list of inliers ids + # dist_pt = ( + # plane_eq[0] * xyz_matrix[:, 0] + plane_eq[1] * xyz_matrix[:, 1] + plane_eq[2] * xyz_matrix[:, 2] + plane_eq[3] + # ) / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2) + + dist_pt = np.dot(xyz_matrix, vecC) + plane_eq[3] + + # Select indexes where distance is biggers than the threshold + pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0] + if len(pt_id_inliers) > len(best_inliers): + best_eq = plane_eq + best_inliers = pt_id_inliers + + #self.inliers = best_inliers + #self.equation = best_eq + + # rtansform to pose output + #tvec = xyz_matrix[best_inliers,:].mean(axis=0) + #pts_best = xyz_matrix[best_inliers,:] - tvec + tvec = xyz_matrix.mean(axis=0) + pts_best = xyz_matrix - tvec + vnorm = np.array(best_eq[:3]) + + # checking error + err = np.dot(pts_best, vnorm) + err_std = err.std() + log.info('Fit error ransac: %s' %str(err_std)) + + img_mean = tvec[2] #z_est.mean() + img_std = err_std + self.plane_params = vnorm.flatten() + self.plane_center = tvec.flatten() + + #log.info(f'Plane : {self.plane_params}, error {img_std:.3f}, step {step_size}') + + return img_mean, img_std + + def fit_plane_ransac_and_grow(self, img_full): + + """ + Find the best equation for a plane of the predefined ROI and then grow the ROI + """ + h,w = img_full.shape[:2] + # start from the original ROI + if self.img_mask is None: + isOk = self.init_image(img_full) + + img_mean, img_std = self.fit_plane_ransac(img_full) + + # make sure that mask is not empty + x0, y0, x1, y1 = self.rect + self.img_mask[y0:y1,x0:x1] = 1 + + # grow the mask + y,x = np.where(self.img_mask > 0.7) + y_min, y_max = y.min(), y.max() + x_min, x_max = x.min(), x.max() + y_min, y_max = np.maximum(0,y_min-1), np.minimum(self.img_mask.shape[0],y_max+1) + x_min, x_max = np.maximum(0,x_min-1), np.minimum(self.img_mask.shape[1],x_max+1) + + # extract ROI + roi_rect = [x_min, y_min, x_max, y_max] + #img_roi = img_full[y_min:y_max,x_min:x_max].astype(np.float32) + xyz_matrix = self.convert_roi_to_points(img_full, point_num = 5000, step_size = 1, roi_rect = roi_rect) + + # check against the plane : do not substract plane.center from all the points + vecC = self.plane_params[:3] + dist_offset = np.dot(self.plane_center, vecC) + dist_pt = np.dot(xyz_matrix, vecC) - dist_offset + + # Select indexes where distance is biggers than the threshold + thresh = 1.5 + err = np.abs(dist_pt) + i2 = np.where( err <= thresh)[0] + + + # transfer xi,yi coordinates to the original image index + ii = self.matrix_index[i2] # convert to 2D index + + # # Convert 2D index to flat index: + # row, col = 2, 1 + # flat_index = np.ravel_multi_index((row, col), arr.shape) # flat_index = 9 + + # # Convert flat index back to 2D index: + # row, col = np.unravel_index(flat_index, arr.shape) # (2, 1) + + self.img_mask.flat[ii] = self.img_mask.flat[ii] + 0.1*(1 - self.img_mask.flat[ii]) + + + # position in 2d array + # unravel_index(a.argmax(), a.shape) + + # output + img_std = err.std() + img_mean = xyz_matrix[i2].mean(axis=0)[2] + + + return img_mean, img_std + + def fit_and_split_roi_recursively(self, roi, level = 0): + # splits ROI on 4 regions and recursevly call + x0,y0,x1,y1 = roi + #roi3d = self.img_xyz[y0:y1,x0:x1,:] + log.info('Processing level %d, region x = %d, y = %d' %(level,x0,y0)) + # check the current fit + roi_params_f = self.fit_plane(roi) + roi_params_ret = [roi_params_f] + if roi_params_f['error'] < self.MIN_STD_ERROR: + log.info('Fit is good enough x = %d, y = %d' %(x0,y0)) + return roi_params_ret + + # too small exit + xs, ys = int((x1 + x0)/2), int((y1 + y0)/2) + if (xs - x0) < self.MIN_SPLIT_SIZE or (ys - y0) < self.MIN_SPLIT_SIZE: + log.info('Min size is reached x = %d, y = %d' %(x0,y0)) + return roi_params_ret + + # 4 ROIs - accept the split if error of one of them is lower from the total + roi_params_list = [] + roi_split = [[x0,y0,xs,ys],[x0,ys,xs,y1],[xs,y0,x1,ys],[xs,ys,x1,y1]] + for roi_s in roi_split: + roi_params_prev = self.fit_and_split_roi_recursively(roi_s, level + 1) + # save locally + #roi_params_list.append(roi_params_prev) + roi_params_list = roi_params_list + roi_params_prev + + # extract each of the below and check the error + makeTheSplit = False + for roi_params_s in roi_params_list: + #roi_params_s = roi_params_prev[-1] + # accept the split if twice lower (if noise of 4 split should be 2) + if roi_params_s['error'] < roi_params_f['error']/2: + makeTheSplit = True + break + + # decide what to return + if makeTheSplit: + roi_params_ret = roi_params_list + log.info('Split at level %d, region x = %d, y = %d' %(level,x0,y0)) + else: + log.info('No split level %d, region x = %d, y = %d' %(level,x0,y0)) + + return roi_params_ret + + def find_planes(self, img): + "finds planes using different algo" + + img_roi = self.preprocess(img) + detect_type = self.detect_type.upper() + + img_mean, img_std = 0,0 + if detect_type == 'P': + img_mean, img_std = self.fit_plane_svd(img) + elif detect_type == 'O': + img_mean, img_std = self.fit_plane_svd_old(img_roi) + #img_mean, img_std = self.fit_plane_with_outliers(img_roi) + elif detect_type == 'R': + img_mean, img_std = self.fit_plane_ransac(img_roi) + elif detect_type == 'G': + img_mean, img_std = self.fit_plane_ransac_and_grow(img) + + + #log.debug(f'camera noise - roi mean : {img_mean}') + self.img_mean = img_mean # final measurements per frame + self.img_std = img_std + return True + + def process_frame(self, img): + "process the entire image and find the planes" + + img_roi = self.preprocess(img) + img3d = self.init_img3d(img_roi) + imgXYZ = self.compute_img3d(img_roi) + roim,rois = self.fit_plane_with_outliers(img_roi) + pose = self.convert_plane_params_to_pose() + + return pose + +if __name__ == '__main__': + #print(__doc__) + p = PlaneDetector() + + + + diff --git a/wrappers/python/applications/planes/test/test_plane_detector.py b/wrappers/python/applications/planes/test/test_plane_detector.py new file mode 100644 index 00000000000..e245e33f932 --- /dev/null +++ b/wrappers/python/applications/planes/test/test_plane_detector.py @@ -0,0 +1,717 @@ +#!/usr/bin/env python + +''' +Tester for multi planar plain detector +================== + +Using depth image to compute depth planes locally for specific ROI. + + +Usage: + +Environemt : + ..\\Envs\\barcode + +Install : + + + +''' + +import sys +import numpy as np +import cv2 as cv +import unittest +from scipy.spatial.transform import Rotation as Rot +import matplotlib.pyplot as plt + + +# importing common Use modules +sys.path.append(r'wrappers\python\applications\planes\src') +from plane_detector import PlaneDetector + +sys.path.append(r'wrappers\python\applications\utils\src') +from opencv_realsense_camera import RealSense, draw_str +from logger import log + + +#%% Helpers +def draw_axis(img, rvec, tvec, cam_mtrx, cam_dist, len = 10): + # unit is mm + points = np.float32([[len, 0, 0], [0, len, 0], [0, 0, len], [0, 0, 0]]).reshape(-1, 3) + axisPoints, _ = cv.projectPoints(points, rvec, tvec, cam_mtrx, cam_dist) + axisPoints = axisPoints.squeeze().astype(np.int32) + img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0,0,255), 3) + img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0,255,0), 3) + img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255,0,0), 3) + return img + +def draw_polygon(img, rvec, tvec, cam_mtrx, cam_dist, points3d): + # unit is mm + points = np.float32(points3d).reshape(-1, 3) + polygon_points, _ = cv.projectPoints(points, rvec, tvec, cam_mtrx, cam_dist) + polygon_points = polygon_points.squeeze().astype(np.int32) + img = cv.polylines(img, [polygon_points], True, (0, 200, 200), 1) + + # To fill the polygon, use thickness=-1 + # cv2.fillPoly(img, [pts], color) + + return img + +def draw_cube(img, corners, imgpts): + imgpts = np.int32(imgpts).reshape(-1,2) + # draw ground floor in green + img = cv.drawContours(img, [imgpts[:4]],-1,(0,255,0),-3) + # draw pillars in blue color + for i,j in zip(range(4),range(4,8)): + img = cv.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255),3) + + # draw top layer in red color + img = cv.drawContours(img, [imgpts[4:]],-1,(0,0,255),3) + return img + +#%% ROI selector from OpenCV +class RectSelector: + def __init__(self, win, callback): + self.win = win + self.callback = callback + cv.setMouseCallback(win, self.onmouse) + self.drag_start = None + self.drag_rect = None + def onmouse(self, event, x, y, flags, param): + x, y = np.int16([x, y]) # BUG + if event == cv.EVENT_LBUTTONDOWN: + self.drag_start = (x, y) + return + if self.drag_start: + if flags & cv.EVENT_FLAG_LBUTTON: + xo, yo = self.drag_start + x0, y0 = np.minimum([xo, yo], [x, y]) + x1, y1 = np.maximum([xo, yo], [x, y]) + self.drag_rect = None + if x1-x0 > 0 and y1-y0 > 0: + self.drag_rect = (x0, y0, x1, y1) + else: + rect = self.drag_rect + self.drag_start = None + self.drag_rect = None + if rect: + self.callback(rect) + def draw(self, vis): + if not self.drag_rect: + return False + x0, y0, x1, y1 = self.drag_rect + cv.rectangle(vis, (x0, y0), (x1, y1), (0, 255, 0), 2) + return True + @property + def dragging(self): + return self.drag_rect is not None + +#%% Data Generator +class DataGen: + def __init__(self): + + self.frame_size = (1280,720) + self.img = None + self.rect = None # roi + + + def add_noise(self, img_gray, noise_percentage = 0.01): + "salt and pepper noise" + if noise_percentage < 0.001: + return img_gray + + + # Get the image size (number of pixels in the image). + img_size = img_gray.size + + # Set the percentage of pixels that should contain noise + #noise_percentage = 0.1 # Setting to 10% + + # Determine the size of the noise based on the noise precentage + noise_size = int(noise_percentage*img_size) + + # Randomly select indices for adding noise. + random_indices = np.random.choice(img_size, noise_size) + + # Create a copy of the original image that serves as a template for the noised image. + img_noised = img_gray.copy() + + # Create a noise list with random placements of min and max values of the image pixels. + #noise = np.random.choice([img_gray.min(), img_gray.max()], noise_size) + noise = np.random.choice([-10, 10], noise_size) + + # Replace the values of the templated noised image at random indices with the noise, to obtain the final noised image. + img_noised.flat[random_indices] += noise + + log.info('adding image noise') + return img_noised + + def init_image(self, img_type = 1): + # create some images for test + w,h = self.frame_size + if img_type == 1: # / + + self.img = np.tile(np.linspace(100, 300, w), (h,1)) + + elif img_type == 2: # /|/ + + self.img = np.tile(np.linspace(100, 200, int(w/2)), (h,2)) + + elif img_type == 3: # |_| + + self.img = np.tile(np.linspace(100, 200, h).reshape((-1,1)), (1,w)) + + elif img_type == 4: # /\ + + self.img = np.tile(np.hstack((np.linspace(300, 500, w>>1),np.linspace(500, 300, w>>1))), (h,1)) + + elif img_type == 5: # dome + + x,y = np.meshgrid(np.arange(w),np.arange(h)) + self.img = (np.abs(x - w/2) + np.abs(y - h/2))/10 + 200 # less slope + + elif img_type == 6: # sphere + + x,y = np.meshgrid(np.arange(w),np.arange(h)) + self.img = np.sqrt((x - w/2)**2 + (y - h/2)**2)/10 + 200 # less slope + + elif img_type == 7: # stair + + x,y = np.meshgrid(np.arange(w),np.arange(h)) + self.img = (np.sign(x - w/2) + np.sign(y - h/2))*5 + 200 # less slope + + + elif img_type == 8: # corner + + x,y = np.meshgrid(np.arange(w),np.arange(h)) + self.img = np.ones((h,w))*250 + img_bool = np.logical_and((x - w/2) < 0, (y - h/2) < 0) + self.img[img_bool] = 230 # quarter + + elif img_type == 10: # flat + + self.img = np.ones((h,w))*500 + + elif img_type == 11: + "chess board" + fname = r"C:\Users\udubin\Documents\Code\opencv-4x\samples\data\left04.jpg" + self.img = cv.imread(fname) + + elif img_type == 12: + self.img = cv.imread('image_scl_001.png', cv.IMREAD_GRAYSCALE) + #self.img = cv.resize(self.img , dsize = self.frame_size) + + elif img_type == 13: + self.img = cv.imread(r"wrappers\python\applications\planes\data\image_ddd_000.png", cv.IMREAD_GRAYSCALE) + #self.img = cv.resize(self.img , dsize = self.frame_size) + + elif img_type == 21: + self.img = cv.imread(r"C:\Data\Depth\Plane\image_scl_000.png", cv.IMREAD_GRAYSCALE) + #self.img = cv.resize(self.img , dsize = self.frame_size) + + #self.img = np.uint8(self.img) + + self.img = self.add_noise(self.img, 0) + self.frame_size = self.img.shape[:2] + return self.img + + def init_roi(self, test_type = 1): + "load the test case" + roi = [0,0,self.frame_size[0],self.frame_size[1]] + if test_type == 1: + roi = [310,230,330,250] # xlu, ylu, xrb, yrb + elif test_type == 2: + roi = [300,220,340,260] # xlu, ylu, xrb, yrb + elif test_type == 3: + roi = [280,200,360,280] # xlu, ylu, xrb, yrb + elif test_type == 4: + roi = [220,140,420,340] # xlu, ylu, xrb, yrb + elif test_type == 4: + roi = [200,120,440,360] # xlu, ylu, xrb, yrb + return roi + + def test_image(self): + "test single image depth" + img = self.init_image(1) + roi = self.init_roi(1) + +#%% Adds display functionality to the PlaneDetector +class PlaneDetectorDisplay(PlaneDetector): + def __init__(self, detect_type='p'): + super().__init__(detect_type) + self.detect_type = detect_type + + self.frame_size = (1280,720) + self.img = None + + def show_image_with_axis(self, img, poses = []): + "draw results : axis on the image. poses are list of 6D vectors" + axis_number = len(poses) + if axis_number < 1: + log.error('No poses found') + + # deal with black and white + img_show = np.uint8(img) #.copy() + if len(img.shape) < 3: + img_show = cv.applyColorMap(img_show, cv.COLORMAP_JET) + + for k in range(axis_number): + + rvec = poses[k][3:] # orientation in degrees + tvec = poses[k][:3] #np.array(, dtype = np.float32).reshape(rvec.shape) # center of the patch + img_show= draw_axis(img_show, rvec, tvec, self.cam_matrix, self.cam_distort, len = 10) + + cv.imshow('Image & Axis', img_show) + log.info('show done') + ch = cv.waitKey() + + def show_image_with_rois(self, img, roi_params_ret = []): + "draw results by projecting ROIs on image" + + axis_number = len(roi_params_ret) + if axis_number < 1: + print('No poses found') + + # deal with black and white + img_show = np.uint8(img) #.copy() + if len(img.shape) < 3: + img_show = cv.applyColorMap(img_show, cv.COLORMAP_JET) + + for roi_p in roi_params_ret: + + pose = self.convert_roi_params_to_pose(roi_p) + + avec = pose[3:6] # orientation in degrees + levl = pose[6] # level + #R = eulerAnglesToRotationMatrix(avec) + R = Rot.from_euler('zyx',avec, degrees = True).as_matrix() + rvec, _ = cv.Rodrigues(R) + tvec = np.array(pose[:3], dtype = np.float32).reshape(rvec.shape) # center of the patch + img_show= draw_axis(img_show, rvec, tvec, self.cam_matrix, self.cam_distort, len = levl) + + cv.imshow('Image & Axis', img_show) + log.info('show done') + ch = cv.waitKey() + + def show_points_3d_with_normal(self, img3d, pose = None): + "display in 3D" + fig = plt.figure() + ax = fig.add_subplot(projection='3d') + + #xs,ys,zs = img3d[:,:,0].reshape((-1,1)), img3d[:,:,1].reshape((-1,1)), img3d[:,:,2].reshape((-1,1)) + + xs,ys,zs = img3d[:,0].reshape((-1,1)), img3d[:,1].reshape((-1,1)), img3d[:,2].reshape((-1,1)) + ax.scatter(xs, ys, zs, marker='.') + + if pose is not None: + pose = pose.flatten() + vnorm = pose[3:6].flatten()*10 + xa, ya, za = [pose[0], pose[0]+vnorm[0]], [pose[1], pose[1]+vnorm[1]], [pose[2], pose[2]+vnorm[2]] + ax.plot(xa, ya, za, 'r', label='Normal') + + + ax.set_xlabel('X [mm]') + ax.set_ylabel('Y [mm]') + ax.set_zlabel('Z [mm]') + ax.set_aspect('equal', 'box') + plt.show() + + def show_rois_3d_with_normals(self, roi_params_ret = [], roi_init = None): + "display in 3D each ROI region with split" + + if len(roi_params_ret) < 1: + log.info('roi_params_ret is empty') + return + + # extract the initial ROI - to make the show more compact + roi_init = [0,0,self.frame_size[1], self.frame_size[0]] if roi_init is None else roi_init + x0,y0,x1,y1 = roi_init + + if self.img_xyz is None: + log.info('Need init') + return + + img3d = self.img_xyz[y0:y1,x0:x1,:] + xs,ys,zs = img3d[:,:,0].reshape((-1,1)), img3d[:,:,1].reshape((-1,1)), img3d[:,:,2].reshape((-1,1)) + + fig = plt.figure() + ax = fig.add_subplot(projection='3d') + ax.scatter(xs, ys, zs, marker='.') + + for roi_p in roi_params_ret: + pose = self.convert_roi_params_to_pose(roi_p) + pose = pose.flatten() + # R = Rot.from_euler('zyx',pose[3:6],degrees=True).as_matrix() + # vnorm = R[:,2]*pose[6] + vnorm = pose[3:6]*pose[6] + #log.info(str(vnorm)) + xa, ya, za = [pose[0], pose[0]+vnorm[0]], [pose[1], pose[1]+vnorm[1]], [pose[2], pose[2]+vnorm[2]] + ax.plot(xa, ya, za, 'r', label='Normal') + + + ax.set_xlabel('X [mm]') + ax.set_ylabel('Y [mm]') + ax.set_zlabel('Z [mm]') + ax.set_aspect('equal', 'box') + plt.show() #block=False) + + def show_axis(self, vis): + "draw axis after plane estimation" + if self.plane_params is None: + return vis + + #rvec = self.plane_params/np.sum(self.plane_params**2) # normalize + rvec = self.convert_plane_params(self.plane_params) + #rvec = self.convert_plane_to_rvec(self.plane_params) + + tvec = self.plane_center + vis = draw_axis(vis, rvec, tvec, self.cam_matrix, self.cam_distort, len = 50) + return vis + + def show_text(self, vis): + "draw text plane estimation" + err_mean, err_std = self.img_mean, self.img_std + if err_mean is None: + return vis + + if self.rect is None: + return vis + + x0, y0, x1, y1 = self.rect + txt = f'{self.detect_type}:{err_mean:.2f}:{err_std:.3f}' + if self.detect_type == 'F': + txt = f'{self.detect_type}:{self.img_fill:.2f} %' + vis = draw_str(vis,(x0,y0-10),txt) + + return vis + + def show_rect_and_text(self, vis): + "draw axis after plane estimation" + err_mean, err_std = self.img_mean, self.img_std + if err_mean is None: + return vis + + if self.rect is None: + return vis + + x0, y0, x1, y1 = self.rect + clr = (0, 0, 0) if vis[y0:y1,x0:x1].mean() > 128 else (240,240,240) + vis = cv.rectangle(vis, (x0, y0), (x1, y1), clr, 2) + txt = f'{self.detect_type}:{err_mean:.2f}-{err_std:.3f}' + if self.detect_type == 'F': + txt = f'{self.detect_type}:{self.img_fill:.2f} %' + vis = draw_str(vis,(x0,y0-10),txt) + + return vis + + def show_rect_and_axis_projected(self, vis): + "projects rectangle on the plane" + if self.rect is None: + return vis + if self.plane_params is None: + return vis + + rvec = self.convert_plane_params(self.plane_params) + tvec = self.plane_center + + vis = draw_axis(vis, rvec, tvec, self.cam_matrix, self.cam_distort, len = 50) + vis = draw_polygon(vis, rvec, tvec, self.cam_matrix, self.cam_distort, self.rect_3d) + + return vis + + def show_mask(self, img): + "draw image mask" + + # deal with black and white + img_show = np.uint8(img) #.copy() + if len(img.shape) < 3: + img_show = cv.applyColorMap(img_show, cv.COLORMAP_JET) + + if not np.all(self.img_mask.shape[:2] == img_show.shape[:2]): + log.error('mask and image size are not equal') + return img_show + + img_show[self.img_mask == 1] = self.color_mask + return img_show + + def show_scene(self, vis): + "draw ROI and Info" + + #vis = self.show_rect_and_text(vis) + #vis = self.show_axis(vis) + + vis = self.show_rect_and_axis_projected(vis) + vis = self.show_text(vis) + + vis = self.show_mask(vis) + + return vis + + +# ---------------------- +#%% Tests +class TestPlaneDetector(unittest.TestCase): + + def test_image_show(self): + "checking image show" + d = DataGen() + img = d.init_image(1) + p = PlaneDetectorDisplay() + poses = [[0,0,100,0,0,45,10]] + p.show_image_with_axis(img,poses) + self.assertFalse(d.img is None) + + def test_init_img3d(self): + "XYZ point cloud structure init" + d = DataGen() + img = d.init_image(1) + p = PlaneDetectorDisplay() + isOk = p.init_image(img) + img3d = p.init_img3d() + self.assertFalse(img3d is None) + + def test_compute_img3d(self): + "XYZ point cloud structure init and compute" + d = DataGen() + img = d.init_image(1) + p = PlaneDetectorDisplay() + img3d = p.init_img3d(img) + imgXYZ = p.compute_img3d(img) + self.assertFalse(imgXYZ is None) + + def test_show_img3d(self): + "XYZ point cloud structure init and compute" + d = DataGen() + img = d.init_image(1) + p = PlaneDetectorDisplay() + img3d = p.init_img3d(img) + imgXYZ = p.compute_img3d(img) + roi = p.init_roi(1) + x0,y0,x1,y1 = roi + roiXYZ = imgXYZ[y0:y1,x0:x1,:] + p.show_points_3d_with_normal(roiXYZ) + self.assertFalse(imgXYZ is None) + + def test_fit_plane_svd(self): + "computes normal to the ROI" + d = DataGen() + img = d.init_image(5) + p = PlaneDetectorDisplay() + roi = p.init_roi(4) + img_roi = p.preprocess(img) + roim,rois = p.fit_plane_svd(img_roi) + pose = p.convert_plane_params_to_pose() + p.show_image_with_axis(img, pose) + p.show_points_3d_with_normal(p.matrix_xyz, pose) + self.assertTrue(pose[0][2] > 0.01) + + def test_fit_plane_depth_image(self): + "computes normal to the ROI" + d = DataGen() + img = d.init_image(13) + p = PlaneDetectorDisplay() + roi = p.init_roi(4) + img_roi = p.preprocess(img) + roim,rois = p.fit_plane_svd(img_roi) + pose = p.convert_plane_params_to_pose() + p.show_image_with_axis(img, pose) + p.show_points_3d_with_normal(p.matrix_xyz, pose) + self.assertTrue(pose[0][2] > 0.01) + + def test_fit_plane_with_outliers(self): + "computes normal to the ROI" + d = DataGen() + img = d.init_image(13) + p = PlaneDetectorDisplay() + roi = p.init_roi(4) + img_roi = p.preprocess(img) + roim,rois = p.fit_plane_with_outliers(img_roi) + pose = p.convert_plane_params_to_pose() + p.show_image_with_axis(img, pose) + p.show_points_3d_with_normal(p.matrix_xyz, pose) + self.assertTrue(pose[0][2] > 0.01) + + def test_fit_plane_ransac(self): + "computes with ransac" + d = DataGen() + img = d.init_image(6) + p = PlaneDetectorDisplay() + roi = p.init_roi(4) + img_roi = p.preprocess(img) + roim,rois = p.fit_plane_ransac(img_roi) + pose = p.convert_plane_params_to_pose() + p.show_image_with_axis(img, pose) + p.show_points_3d_with_normal(p.matrix_xyz, pose) + self.assertTrue(pose[0][2] > 0.01) + + def test_split_roi(self): + "computes ROIS and splits if needed" + p = PlaneDetector() + p.MIN_STD_ERROR = 0.1 + img = p.init_image(13) + roi = p.init_roi(4) + img3d = p.init_img3d(img) + imgXYZ = p.compute_img3d(img) + roi_list= p.fit_and_split_roi_recursively(roi) + p.show_rois_3d_with_normals(roi_list, roi) + p.show_image_with_rois(p.img, roi_list) + + for roi_s in roi_list: + self.assertFalse(roi_s['error'] > 0.01) + +#%% Run Test +def RunTest(): + #unittest.main() + suite = unittest.TestSuite() + #suite.addTest(TestPlaneDetector("test_image_show")) # ok + #suite.addTest(TestPlaneDetector("test_init_img3d")) # ok + #suite.addTest(TestPlaneDetector("test_compute_img3d")) # ok + #suite.addTest(TestPlaneDetector("test_show_img3d")) # ok + + #suite.addTest(TestPlaneDetector("test_fit_plane_svd")) # ok + #suite.addTest(TestPlaneDetector("test_fit_plane_depth_image")) # + #suite.addTest(TestPlaneDetector("test_fit_plane_with_outliers")) + suite.addTest(TestPlaneDetector("test_fit_plane_ransac")) + + #suite.addTest(TestPlaneDetector("test_split_roi")) + + + + runner = unittest.TextTestRunner() + runner.run(suite) + +# ---------------------- +#%% App +class PlaneApp: + def __init__(self): + self.cap = RealSense() # + self.cap.set_display_mode('d16') + self.cap.set_exposure(5000) + self.frame = None + self.rect = None + self.paused = False + self.trackers = [] + + self.show_dict = {} # hist show + + self.detect_type = 'P' + self.show_type = 'depth' # left, depth + self.win_name = 'Plane Detector (q-quit, c-clear, a,r,p,o,g,s)' + + cv.namedWindow(self.win_name ) + self.rect_sel = RectSelector(self.win_name , self.on_rect) + + def on_rect(self, rect): + "remember ROI defined by user" + #self.define_roi(self.frame, rect) + tracker = PlaneDetectorDisplay() #estimator_type=self.estim_type, estimator_id=estim_ind) + tracker.rect = rect + tracker.detect_type = self.detect_type + self.trackers.append(tracker) + log.info(f'Adding plane estimator at : {rect}') + + def process_image(self, img_depth): + "makes measurements" + for tracker in self.trackers: + tracker.find_planes(img_depth) + + def show_scene(self, frame): + "draw ROI and Info" + if self.show_type == 'left': + vis = frame[:,:,0].astype(np.uint8) + else: + vis = cv.convertScaleAbs(frame[:,:,2], alpha=0.1).astype(np.uint8) + + vis = cv.cvtColor(vis, cv.COLOR_GRAY2BGR) + self.rect_sel.draw(vis) + + for tracker in self.trackers: + vis = tracker.show_scene(vis) + + return vis + + def show_histogram(self, img): + "show roi histgram" + if self.rect is None: + #print('define ROI') + return 0 + + x0, y0, x1, y1 = self.rect + img_roi = img[y0:y1,x0:x1].astype(np.float32) + # Compute histogram + hist, bins = np.histogram(img_roi.flatten(), bins=1024, range=[0, 2**15]) + + if not 'fig' in self.show_dict : #len(self.show_dict) < 1: + fig, ax = plt.subplots() + fig.set_size_inches([24, 16]) + ax.set_title('Histogram (Depth)') + ax.set_xlabel('Bin') + ax.set_ylabel('Frequency') + lineGray, = ax.plot(bins[:-1], hist, c='k', lw=3) + ax.set_xlim(bins[0], bins[-1]) + ax.set_ylim(0, max(hist)+10) + plt.ion() + #plt.show() + + self.show_dict = {'fig':fig, 'ax':ax, 'line':lineGray} + else: + self.show_dict['line'].set_ydata(hist) + + self.show_dict['fig'].canvas.draw() + return + + def run(self): + while True: + playing = not self.paused and not self.rect_sel.dragging + if playing or self.frame is None: + ret, frame = self.cap.read() + if not ret: + break + self.frame = frame.copy() + + + #self.statistics(frame) + img_depth = self.frame[:,:,2] + self.process_image(img_depth) + + #self.show_histogram(frame) + + vis = self.show_scene(frame) + cv.imshow(self.win_name , vis) + ch = cv.waitKey(1) + if ch == ord(' '): + self.paused = not self.paused + elif ch == ord('a'): + self.detect_type = 'A' + log.info(f'Detect type : {self.detect_type}') + elif ch == ord('r'): + self.detect_type = 'R' + log.info(f'Detect type : {self.detect_type}') + elif ch == ord('p'): + self.detect_type = 'P' + log.info(f'Detect type : {self.detect_type}') + elif ch == ord('o'): + self.detect_type = 'O' + log.info(f'Detect type : {self.detect_type}') + elif ch == ord('g'): + self.detect_type = 'G' + log.info(f'Detect type : {self.detect_type}') + elif ch == ord('s'): + self.show_type = 'left' if self.show_type == 'depth' else 'depth' + log.info(f'Show type : {self.show_type}') + if ch == ord('c'): + if len(self.trackers) > 0: + t = self.trackers.pop() + if ch == 27 or ch == ord('q'): + break + + +if __name__ == '__main__': + #print(__doc__) + + #RunTest() + PlaneApp().run() + + + diff --git a/wrappers/python/applications/pose/README.md b/wrappers/python/applications/pose/README.md new file mode 100644 index 00000000000..138e7c114e4 --- /dev/null +++ b/wrappers/python/applications/pose/README.md @@ -0,0 +1,53 @@ +![Pose6D](doc/pose6d-cup.gif) + +[![image](https://img.shields.io/pypi/v/scikit-spatial.svg)](https://pypi.python.org/pypi/scikit-spatial) + + + +# Introduction + +This demo shows implementation of 6 degree of freedom (DoF) object detcetion and pose estimation based on RGB data of known object. + + +- 6D Pose of a Chess board : estimates translation and rotation of the chess pattern +- 6D Pose of a Aruco marker : estimates translation and rotation of the aruco marker +- 6D Pose of a Known object : estimates translation and rotation of a knwon objects + +The camera calibration parameters are known. If the camera position is also known this code could be used to control robots and automation processes. This code could be easily integrated in your robotics and video processing pipe line. + +# 6 DoF Pose Estimation + +Examples of the pose estimation utilities. + +Example Image | Type | Explanations | +:------------: | :----------: | :----------: | +![detecting chess board](doc/chess.gif) | Chess Board | RGB based chess board pose estimation | +![aruco martker pose](doc/cam_d_3.jpg) | Aruco Marker | Aruco marker detection and pose estimation | +![a general object pose](doc/pose6d-cup.gif) | Known Object | cup detection and pose estimation | +![a general object pose](doc/pose6d-matchbox.gif) | Known Object | a match box detection and pose estimation | + +# Modules and License + +For 6 DoF object pose estimation we are using code available from RobotAI ([https://wwww.robotai.info](https://www.robotai.info/)) + +## Installation Windows/Ubuntu + +1. Install Pose6D executable from RobotAI + +## Usage + +1. Install Pose6D executable: + +2. Connect to RealSense camera + +3. Acquire dataset of the object you want to detect + +4. Label the data using RobotAI labeling tool + +5. Train AI model + +6. Run Pose6D software to detect the object in real time + +## Usage + +Start Pose6D server and connect to it. diff --git a/wrappers/python/applications/pose/doc/chess.gif b/wrappers/python/applications/pose/doc/chess.gif new file mode 100644 index 00000000000..b46cd143d3d Binary files /dev/null and b/wrappers/python/applications/pose/doc/chess.gif differ diff --git a/wrappers/python/applications/pose/doc/object_0007.gif b/wrappers/python/applications/pose/doc/object_0007.gif new file mode 100644 index 00000000000..4816f029c08 Binary files /dev/null and b/wrappers/python/applications/pose/doc/object_0007.gif differ diff --git a/wrappers/python/applications/pose/doc/pose6d-cup.gif b/wrappers/python/applications/pose/doc/pose6d-cup.gif new file mode 100644 index 00000000000..cac167b211a Binary files /dev/null and b/wrappers/python/applications/pose/doc/pose6d-cup.gif differ diff --git a/wrappers/python/applications/pose/doc/pose6d-matchbox.gif b/wrappers/python/applications/pose/doc/pose6d-matchbox.gif new file mode 100644 index 00000000000..ae1c622d442 Binary files /dev/null and b/wrappers/python/applications/pose/doc/pose6d-matchbox.gif differ diff --git a/wrappers/python/applications/robots/README.md b/wrappers/python/applications/robots/README.md new file mode 100644 index 00000000000..e4ca742149f --- /dev/null +++ b/wrappers/python/applications/robots/README.md @@ -0,0 +1,53 @@ +![Robots](doc/robot_types.png) + + +# Introduction + +This is a collection of the examples to facilitate camera robot integration. + +- moving camera : cameras mounted on a robotic arm +- on board camera : camera mounted on AGV/AMR mobile platform +- stationary camera : camera mounted on the fixed structure + +This code could be integrated in your robotics and video processing pipe line. + + +## Hand-Eye Calibration + + +The dataset: + + +| Robot pose and image | Robot pose and image | Robot pose and image | +|:------------: | :----------: | :-------------: | +![example](data/calibration_hand_eye/calib_robot_0001.jpg) | ![example](data/calibration_hand_eye/calib_robot_0001.jpg) | ![example](data/calibration_hand_eye/calib_robot_0002.jpg) | +![example](data/calibration_hand_eye/calib_robot_0003.jpg) | ![example](data/calibration_hand_eye/calib_robot_0004.jpg) | ![example](data/calibration_hand_eye/calib_robot_0005.jpg) | +![example](data/calibration_hand_eye/calib_robot_0006.jpg) | ![example](data/calibration_hand_eye/calib_robot_0007.jpg) | ![example](data/calibration_hand_eye/calib_robot_0008.jpg) | + +Json file that describes robot arm positions for each image above is here (data/calibration_hand_eye/camera_robot_calibration.json) and looks like this: + +```json + +{"imageName": "calib_robot_0001.jpg", "robotPose": [127.763,355.406,5.45674,-0.1392,-3.05356,-0.138536]}, +{"imageName": "calib_robot_0002.jpg", "robotPose": [131.589,356.447,5.63756,-0.005102,-3.0533,-0.133261]}, +{"imageName": "calib_robot_0003.jpg", "robotPose": [135.368,357.804,5.62477,0.127749,-3.04725,-0.127267]}, +{"imageName": "calib_robot_0004.jpg", "robotPose": [128.667,362.151,9.00709,-0.133892,-3.05282,-0.005054]}, +{"imageName": "calib_robot_0005.jpg", "robotPose": [131.855,363.293,9.13366,0.000735,-3.05593,-9e-06]}, +{"imageName": "calib_robot_0006.jpg", "robotPose": [135.003,364.624,9.10671,0.133818,-3.05301,0.005782]}, +{"imageName": "calib_robot_0007.jpg", "robotPose": [129.61,369.24,12.0069,-0.128087,-3.04711,0.127505]}, +{"imageName": "calib_robot_0008.jpg", "robotPose": [132.029,370.418,12.0255,0.006511,-3.05289,0.133136]}, +{"imageName": "calib_robot_0009.jpg", "robotPose": [134.556,371.645,11.9296,0.139922,-3.05326,0.139578]}, +{"imageName": "calib_robot_0010.jpg", "robotPose": [121.154,356.088,6.52716,0.136842,3.13027,0.136899]}, +{"imageName": "calib_robot_0011.jpg", "robotPose": [124.967,356.443,6.52796,-0.000734,3.13673,0.13713]}, + + +``` + +## Installation Windows/Ubuntu + +1. + +## Usage + + + diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0001.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0001.jpg new file mode 100644 index 00000000000..311509c9318 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0001.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0002.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0002.jpg new file mode 100644 index 00000000000..bb6acd31dd8 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0002.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0003.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0003.jpg new file mode 100644 index 00000000000..b2b02a3a79b Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0003.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0004.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0004.jpg new file mode 100644 index 00000000000..6d2c0c44529 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0004.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0005.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0005.jpg new file mode 100644 index 00000000000..c744f82c085 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0005.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0006.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0006.jpg new file mode 100644 index 00000000000..f9e6cf774cc Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0006.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0007.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0007.jpg new file mode 100644 index 00000000000..99ae392b5a2 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0007.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0008.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0008.jpg new file mode 100644 index 00000000000..85da529d4f6 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0008.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0009.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0009.jpg new file mode 100644 index 00000000000..09a57fb11ab Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0009.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0010.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0010.jpg new file mode 100644 index 00000000000..975283c50d8 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0010.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0011.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0011.jpg new file mode 100644 index 00000000000..597a37c4744 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0011.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0012.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0012.jpg new file mode 100644 index 00000000000..33d46ef472a Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0012.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0013.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0013.jpg new file mode 100644 index 00000000000..09b52de2f2a Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0013.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0014.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0014.jpg new file mode 100644 index 00000000000..8d3481ae28c Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0014.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0015.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0015.jpg new file mode 100644 index 00000000000..d311c88ba64 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0015.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0016.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0016.jpg new file mode 100644 index 00000000000..e49dcd2e823 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0016.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0017.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0017.jpg new file mode 100644 index 00000000000..e38f1a695a2 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0017.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0018.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0018.jpg new file mode 100644 index 00000000000..4a2d1dbc57f Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0018.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0019.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0019.jpg new file mode 100644 index 00000000000..9bf965f8d40 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0019.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0020.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0020.jpg new file mode 100644 index 00000000000..ed82a63fbb1 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0020.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0021.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0021.jpg new file mode 100644 index 00000000000..afe39c5beaa Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0021.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0022.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0022.jpg new file mode 100644 index 00000000000..fc6f71d675f Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0022.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0023.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0023.jpg new file mode 100644 index 00000000000..9c0a5bf7c55 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0023.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0024.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0024.jpg new file mode 100644 index 00000000000..cfca7127457 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0024.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0025.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0025.jpg new file mode 100644 index 00000000000..c47e7658ede Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0025.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0026.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0026.jpg new file mode 100644 index 00000000000..4552b33d1f7 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0026.jpg differ diff --git a/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0027.jpg b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0027.jpg new file mode 100644 index 00000000000..7c017f9d855 Binary files /dev/null and b/wrappers/python/applications/robots/data/calibration_hand_eye/calib_robot_0027.jpg differ diff --git a/wrappers/python/applications/robots/doc/robot_types.png b/wrappers/python/applications/robots/doc/robot_types.png new file mode 100644 index 00000000000..56eb6b4f32c Binary files /dev/null and b/wrappers/python/applications/robots/doc/robot_types.png differ diff --git a/wrappers/python/applications/utils/README.md b/wrappers/python/applications/utils/README.md new file mode 100644 index 00000000000..be3bd40e42a --- /dev/null +++ b/wrappers/python/applications/utils/README.md @@ -0,0 +1,110 @@ +![](data/show_examples.jpg) + +[![image](https://img.shields.io/pypi/v/scikit-spatial.svg)](https://pypi.python.org/pypi/scikit-spatial) + + + +# Introduction + +This module provides some useful utilities and functions for RealSense camera usage. +The object detection is based on RGB values of the camera. +The following functions are supported: + +- OpenCV like camera usage +- RGB and depth information alignments +- Extraction of IR images from left and right sensor +- Saving and recording of the RGB, Depth and RGD data + +These functions are reused in other RealSense object detection modules. + + +# Modules + +- opencv_realsense_camera : camera wrapper with multiple internal features exposed like opencv interface +- camera_noise_estimator : tool to measure depth camera noise + + +# RealSense Camera Wrapper + +- Display Modes - different image data that could be extracted from the camera. + +Display Image | Mode | Explanations | +:------------: | :----------: | :----------: | +![RGB](doc/cam_rgb_d.jpg) | Display - Option 0 | simple RGB output with display control on | +![d16](doc/cam_d_3.jpg) | Display -Option 3 | simple 16 bit data with Left, Right and Depth images stacked as RGB planes | +![sc1](doc/cam_d_5.jpg) | Display - Option 5 | Depth image mapped to a color space using specific scaling factor to fit 12 bit depth to uint8 range | +![ii2](doc/cam_d_9.jpg) | Display - Option 9 | Left Right IR images stacked in x axis | +![ii2](doc/cam_e_2.jpg) | Exposure - Option 2 | reduced exposure value | +![ii2](doc/cam_e_9.jpg) | Exposure - Option 9 | different exposure value | +![ii2](doc/cam_g_5.jpg) | Gain - Option 2 | different camera gain value | +![ii2](doc/cam_p_0.jpg) | Projector - Option 0 | turn projector on-off | + +## Usage + +```py +>>> from opencv_realsense_camera import RealSense + +>>> rs_cap = RealSense('rgb') + +``` + +# Camera Noise Estimator + +Depth Noise measurement tool - a simple UI to measure noise in the depth data. It supports measurement of the temporal noise per pixel, spatial noise per image ROI and fitted plane noise. In addition, it reports the number/percent of the non valid pixels. The results are visualized and alo printed in the console window. +See python file for the usage options. + + +Display Results | Mode | Explanations | +:------------: | :----------: | :----------: | +![RGB](doc/noise_measure_stp.gif) | Option s,t,p | measuring temporal and spatial noise | +![d16](doc/noise_measure.gif) | Option p | measuring plane fit noise - distance from the fitted plane | + + +## Usage + +```py +>>> from test_camera_noise_estimator import NoiseApp + +>>> NoiseApp().run() + +``` + +# Logger + +Logging tool - a python based logger adapted to measure timing and display print information. + + +Display Results | Mode | Explanations | +:------------: | :----------: | :----------: | +![sc1](doc/noise_error_log.jpg) | Option p only | results and error logging | + + +## Usage + +```py +>>> from logger import log + +>>> log.info('Kuku') + +``` + +# Installation Windows + +Python is a self contained development environment. We use PIP to manage the package installation. +You can use Conda, Miniconda or other package managers. + +1. Install python 3.10 from Python Release Python 3.10.0 | + +2. Create virtual environment. In Windows PowerShell: + + python -m venv "your path"\Envs\utils + +3. Activate virtual environment. In Windows CMD shell: + + "your path"\Envs\utils\Scripts\activate.bat + +4. Installing realsense driver. For example, download pyrealsense2-2.55.10.6089-cp310-cp310-win_amd64.whl: + + pip install pyrealsense2-2.55.10.6089-cp310-cp310-win_amd64.whl + + diff --git a/wrappers/python/applications/utils/doc/cam_d_3.jpg b/wrappers/python/applications/utils/doc/cam_d_3.jpg new file mode 100644 index 00000000000..9a571b89865 Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_d_3.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_d_5.jpg b/wrappers/python/applications/utils/doc/cam_d_5.jpg new file mode 100644 index 00000000000..f1a5be97d02 Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_d_5.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_d_9.jpg b/wrappers/python/applications/utils/doc/cam_d_9.jpg new file mode 100644 index 00000000000..e5bb0a289fc Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_d_9.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_e_2.jpg b/wrappers/python/applications/utils/doc/cam_e_2.jpg new file mode 100644 index 00000000000..79bcb262cc4 Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_e_2.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_e_9.jpg b/wrappers/python/applications/utils/doc/cam_e_9.jpg new file mode 100644 index 00000000000..cf88350927c Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_e_9.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_g_5.jpg b/wrappers/python/applications/utils/doc/cam_g_5.jpg new file mode 100644 index 00000000000..0eee2bea403 Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_g_5.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_p_0.jpg b/wrappers/python/applications/utils/doc/cam_p_0.jpg new file mode 100644 index 00000000000..ef1c6569d4c Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_p_0.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_rgb.jpg b/wrappers/python/applications/utils/doc/cam_rgb.jpg new file mode 100644 index 00000000000..93fe4f43351 Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_rgb.jpg differ diff --git a/wrappers/python/applications/utils/doc/cam_rgb_d.jpg b/wrappers/python/applications/utils/doc/cam_rgb_d.jpg new file mode 100644 index 00000000000..e643c3163b2 Binary files /dev/null and b/wrappers/python/applications/utils/doc/cam_rgb_d.jpg differ diff --git a/wrappers/python/applications/utils/doc/imageL_d16_001.png b/wrappers/python/applications/utils/doc/imageL_d16_001.png new file mode 100644 index 00000000000..26250034893 Binary files /dev/null and b/wrappers/python/applications/utils/doc/imageL_d16_001.png differ diff --git a/wrappers/python/applications/utils/doc/imageR_d16_001.png b/wrappers/python/applications/utils/doc/imageR_d16_001.png new file mode 100644 index 00000000000..ac8c2b24eb8 Binary files /dev/null and b/wrappers/python/applications/utils/doc/imageR_d16_001.png differ diff --git a/wrappers/python/applications/utils/doc/image_d16_000.png b/wrappers/python/applications/utils/doc/image_d16_000.png new file mode 100644 index 00000000000..abf41a120b5 Binary files /dev/null and b/wrappers/python/applications/utils/doc/image_d16_000.png differ diff --git a/wrappers/python/applications/utils/doc/image_sc2_000.png b/wrappers/python/applications/utils/doc/image_sc2_000.png new file mode 100644 index 00000000000..7de2a382a0d Binary files /dev/null and b/wrappers/python/applications/utils/doc/image_sc2_000.png differ diff --git a/wrappers/python/applications/utils/doc/noise_error_log.jpg b/wrappers/python/applications/utils/doc/noise_error_log.jpg new file mode 100644 index 00000000000..5dc63dd85a6 Binary files /dev/null and b/wrappers/python/applications/utils/doc/noise_error_log.jpg differ diff --git a/wrappers/python/applications/utils/doc/noise_measure.gif b/wrappers/python/applications/utils/doc/noise_measure.gif new file mode 100644 index 00000000000..b7f3586ecfe Binary files /dev/null and b/wrappers/python/applications/utils/doc/noise_measure.gif differ diff --git a/wrappers/python/applications/utils/doc/noise_measure_stp.gif b/wrappers/python/applications/utils/doc/noise_measure_stp.gif new file mode 100644 index 00000000000..57ff9984519 Binary files /dev/null and b/wrappers/python/applications/utils/doc/noise_measure_stp.gif differ diff --git a/wrappers/python/applications/utils/src/__init__.py b/wrappers/python/applications/utils/src/__init__.py new file mode 100644 index 00000000000..4ab2d285fab --- /dev/null +++ b/wrappers/python/applications/utils/src/__init__.py @@ -0,0 +1,3 @@ +print('utils-src') +from src.logger import log +import opencv_realsense_camera diff --git a/wrappers/python/applications/utils/src/camera_noise_estimator.py b/wrappers/python/applications/utils/src/camera_noise_estimator.py new file mode 100644 index 00000000000..b68fb44ce5a --- /dev/null +++ b/wrappers/python/applications/utils/src/camera_noise_estimator.py @@ -0,0 +1,274 @@ + +''' +Noise Measurement App +================== + +Using depth image to measure noise in specific image ROI. + +Usage +----- + +Environemt : + .\\Envs\\barcode + +Install : + + +''' + +import sys +import numpy as np +import cv2 as cv + +# importing common Use modules +sys.path.append(r'..\utils\src') +from logger import log + + +#%% Main Function +class NoiseEstimator: + def __init__(self, m_type = 'T'): + self.frame = None + self.rect = None + self.img_int_mean = None + self.img_int_std = None + self.roi_type = 0 # 0-entire image + self.measure_type = m_type # type of the measurements + + # plane fit params + self.frame_size = (1280,720) + self.matrix_inv = None # holds inverse params of the + self.matrix_dir = None # direct u,v,1 + self.plane_params = None # rvec not normalized + self.plane_center = None # tvec + self.cam_matrix = None + self.cam_distort = None + + # statistics + self.depth_mean = [] # vector of mean measurements of ROI + self.depth_std = [] # vector of std measurements of ROI + self.depth_true = [] # actual depth values + + self.img_mean = 0 # final measurements per frame + self.img_std = 0 + self.img_fill = 0 # fill rate + + + def init_roi(self, frame, test_type = 14): + "load the roi case" + h,w = frame.shape[:2] + w2, h2 = w>>1, h>>1 + roi = [0,0,w,h] + if test_type == 1: + roi = [w2-3,h2-3,w2+3,h2+3] # xlu, ylu, xrb, yrb + elif test_type == 2: + roi = [300,220,340,260] # xlu, ylu, xrb, yrb + elif test_type == 3: + roi = [280,200,360,280] # xlu, ylu, xrb, yrb + elif test_type == 4: + roi = [220,140,420,340] # xlu, ylu, xrb, yrb + elif test_type == 5: + roi = [200,120,440,360] # xlu, ylu, xrb, yrb + elif test_type == 11: + roi = [w2-16,h2-16,w2+16,h2+16] # xlu, ylu, xrb, yrb + elif test_type == 12: + roi = [w2-32,h2-32,w2+32,h2+32] # xlu, ylu, xrb, yrb + elif test_type == 13: + roi = [w2-64,h2-64,w2+64,h2+64] # xlu, ylu, xrb, yrb + elif test_type == 14: + roi = [w2-64,h2-48,w2+64,h2+48] # xlu, ylu, xrb, yrb + + log.info(f'Using ROI : {roi}') + return roi + + def preprocess(self, img): + "image preprocessing - extracts roi and converts from uint8 to float using log function" + if self.rect is None: # use entire image + self.rect = self.init_roi(img, self.roi_type) + + x0, y0, x1, y1 = self.rect + if len(img.shape) > 2: + img_roi = img[y0:y1,x0:x1,2].astype(np.float32) + else: + img_roi = img[y0:y1,x0:x1].astype(np.float32) + return img_roi + + def temporal_noise(self, img_roi): + "makes integration over ROI and estimates STD of the region" + assert len(img_roi.shape) < 3 , 'image must be 2D - depth only' + + if self.img_int_mean is None: + self.img_int_mean = img_roi + self.img_int_std = np.zeros_like(img_roi) + + # reset if the shape is changed + if not np.all(img_roi.shape == self.img_int_mean.shape): + self.img_int_mean = img_roi + self.img_int_std = np.zeros_like(img_roi) + + valid_bool = img_roi > 0 + #valid_num = valid_bool.sum() + #nr,nc = img_roi.shape[:2] + + self.img_int_mean += 0.1*(img_roi - self.img_int_mean) + self.img_int_std += 0.1*(np.abs(img_roi - self.img_int_mean) - self.img_int_std) + + err_std_valid = self.img_int_std.copy() + err_std_valid[~valid_bool] = 0 #100 + err_std = err_std_valid.mean() + err_mean = self.img_int_mean.mean() + return err_mean, err_std + + def spatial_noise_parallel(self, img_roi): + "estimates mean and std of the flat surface - parallel to the camera" + #assert len(img_roi.shape) < 3 , 'image must be 2D - depth only' + + valid_bool = img_roi > 0 + + self.img_int_mean = img_roi[valid_bool].mean() + self.img_int_std = img_roi[valid_bool].std() + + img_mean = self.img_int_mean + img_std = self.img_int_std + return img_mean, img_std + + def plane_fit_init(self): + "prepares data for real time fit a*x+b*y+c = z" + self.cam_matrix = np.array([[650,0,self.frame_size[0]/2],[0,650,self.frame_size[1]/2],[0,0,1]], dtype = np.float32) + self.cam_distort = np.array([0,0,0,0,0],dtype = np.float32) + + x0,y0,x1,y1 = self.rect + h,w = y1-y0, x1-x0 + x_grid = np.arange(x0, x1, 1) + y_grid = np.arange(y0, y1, 1) + x, y = np.meshgrid(x_grid, y_grid) + + # camera coordinates + xy = np.hstack((x.reshape(-1,1),y.reshape(-1,1))) + xy = np.expand_dims(xy, axis=1).astype(np.float32) + xy_undistorted = cv.undistortPoints(xy, self.cam_matrix, self.cam_distort) + + u = xy_undistorted[:,0,0].reshape((h,w)).reshape(-1,1) + v = xy_undistorted[:,0,1].reshape((h,w)).reshape(-1,1) + + # check + #u, v = u*self.cam_matrix[0,0], v*self.cam_matrix[1,1] + + self.matrix_dir = np.hstack((u,v,u*0+1)) + self.matrix_inv = np.linalg.pinv(self.matrix_dir) + + def plane_fit_noise(self, img_roi): + "estimates mean and std of the plane fit" + n,m = img_roi.shape[:2] + img_roi = img_roi.reshape((-1,1)) + valid_bool = img_roi > 0 + valid_bool = valid_bool.flatten() + #log.info(f'Timing : 1') + + # init params of the inverse + if self.matrix_inv is None: + self.plane_fit_init() + + # plane params - using only valid + z = img_roi[valid_bool] + xyz_matrix = self.matrix_dir[valid_bool,:] + xyz_matrix[:,:3] = xyz_matrix[:,:3]*z # keep 1 intact + xyz_center = xyz_matrix[:,:3].mean(axis=0) + xyz_matrix = xyz_matrix - xyz_center + #log.info(f'Timing : 2') + + # mtrx_dir = np.hstack((self.matrix_dir[valid_bool,0]*z,self.matrix_dir[valid_bool,1]*z,z*0+1)) + # mtrx_inv = np.linalg.pinv(mtrx_dir) + # #mtrx_inv = self.matrix_inv[:,valid_bool] + # plane_params = np.dot(mtrx_inv,z) + + # decimate to make it run faster reduce size of the grid for speed. 1000 pix - 30x30 - step 1, 10000 pix - step=3 + roi_area = n*m + step_size = int(np.sqrt(roi_area)/7) if roi_area > 1000 else 1 + + # using svd to make the fit + U, S, Vh = np.linalg.svd(xyz_matrix[::step_size,:], full_matrices=True) + ii = np.argmin(S) + vnorm = Vh[ii,:] + #log.info(f'Timing : 3') + + # keep orientation + plane_params = vnorm*np.sign(vnorm[2]) + + # estimate error + err = np.dot(xyz_matrix,plane_params) + z_est = z + err + + img_mean = z_est.mean() + img_std = err.std() + self.plane_params = plane_params[:3].flatten() + self.plane_center = xyz_center.flatten() + + log.info(f'Plane : {self.plane_params}, error {img_std:.3f}, step {step_size}') + + return img_mean, img_std + + def estimate_fill_rate(self, img_roi): + "estimates mean and std of the flat surface - parallel to the camera" + #assert len(img_roi.shape) < 3 , 'image must be 2D - depth only' + + non_valid_bool = img_roi < 1 + m,n = img_roi.shape[:2] + fill_rate = non_valid_bool.sum()/(m*n) + + return (1 - fill_rate) # valid fill + + def process_image(self, img): + "makes integration and noise measurement over ROI" + + img_roi = self.preprocess(img) + #err_mean, err_std = self.temporal_noise(img_roi) + img_mean, img_std = self.spatial_noise_parallel(img_roi) + + # this code is used for testing depth fft + self.depth_mean.append(img_mean) + self.depth_std.append(img_std) + + #log.debug(f'camera noise - roi mean : {img_mean}') + + return img_std + + def do_measurements(self, img): + "makes integration and noise measurement over ROI" + + img_roi = self.preprocess(img) + measure_type = self.measure_type.upper() + + img_mean, img_std, fill_rate = 0,0,0 + if measure_type == 'T': + img_mean, img_std = self.temporal_noise(img_roi) + elif measure_type == 'S': + img_mean, img_std = self.spatial_noise_parallel(img_roi) + elif measure_type == 'F': + fill_rate = self.estimate_fill_rate(img_roi) + elif measure_type == 'P': + img_mean, img_std = self.plane_fit_noise(img_roi) + elif measure_type == 'A': + ret = self.show_statistics(img_roi) + #log.debug(f'camera noise - roi mean : {img_mean}') + self.img_mean = img_mean # final measurements per frame + self.img_std = img_std + self.img_fill = fill_rate + return True + + def print_statistics(self, frame): + "just print some info" + ii = np.logical_and(frame > 0.5, frame < 15000) # 65535 - None + + isok = np.any(np.isfinite([frame[ii]])) + minv = frame[ii].min() + maxv = frame[ii].max() + avg = frame[ii].mean() + stdv = frame[ii].std() + locv = ii.sum() #np.any(np.isnan(frame[ii])) + log.info(f'Stat : {minv:.2f}:{avg:.2f}:{maxv:.2f} - std {stdv:.2f}. Has None {locv}, is finite {isok}') + return True + +if __name__ == '__main__': + #print(__doc__) + m = NoiseEstimator() \ No newline at end of file diff --git a/wrappers/python/applications/utils/src/logger.py b/wrappers/python/applications/utils/src/logger.py new file mode 100644 index 00000000000..02cf6d9eb1b --- /dev/null +++ b/wrappers/python/applications/utils/src/logger.py @@ -0,0 +1,74 @@ +import datetime +import logging +import os +import sys + +# import termcolor + +# if os.name == "nt": # Windows +# import colorama +# colorama.init() + + +# COLORS = { +# "WARNING": "yellow", +# "INFO": "white", +# "DEBUG": "blue", +# "CRITICAL": "red", +# "ERROR": "red", +# } + + +# class ColoredFormatter(logging.Formatter): +# def __init__(self, fmt, use_color=True): +# logging.Formatter.__init__(self, fmt) +# self.use_color = use_color + +# def format(self, record): +# levelname = record.levelname +# if self.use_color and levelname in COLORS: + +# def colored(text): +# return termcolor.colored( +# text, +# color=COLORS[levelname], +# attrs={"bold": True}, +# ) + +# record.levelname2 = colored("{:<7}".format(record.levelname)) +# record.message2 = colored(record.msg) + +# asctime2 = datetime.datetime.fromtimestamp(record.created) +# record.asctime2 = termcolor.colored(asctime2, color="green") + +# record.module2 = termcolor.colored(record.module, color="cyan") +# record.funcName2 = termcolor.colored(record.funcName, color="cyan") +# record.lineno2 = termcolor.colored(record.lineno, color="cyan") +# return logging.Formatter.format(self, record) + + +log = logging.getLogger("robot") +log.setLevel(logging.DEBUG) + +if len(log.handlers)<1: + print('Logger activated') + #formatter = logging.Formatter('[%(asctime)s.%(msecs)03d] {%(filename)6s:%(lineno)3d} %(levelname)s - %(message)s', datefmt="%M:%S", style="{") + formatter = logging.Formatter('[%(asctime)s] - [%(filename)28s:%(lineno)4d] - %(levelname)5s - %(message)s') + log.setLevel("DEBUG") + + console_handler = logging.StreamHandler() # sys.stderr + console_handler.setLevel("DEBUG") + console_handler.setFormatter(formatter) + log.addHandler(console_handler) + + # console_handler = logging.StreamHandler(sys.stderr) + # handler_format = ColoredFormatter("%(asctime)s [%(levelname2)s] %(module2)s:%(funcName2)s:%(lineno2)s - %(message2)s") + # console_handler.setFormatter(handler_format) + # log.addHandler(console_handler) + + # file_handler = logging.FileHandler("main_app.log", mode="a", encoding="utf-8") + # file_handler.setLevel("WARNING") + # file_handler.setFormatter(formatter) + # logger.addHandler(file_handler) + + diff --git a/wrappers/python/applications/utils/src/opencv_realsense_camera.py b/wrappers/python/applications/utils/src/opencv_realsense_camera.py new file mode 100644 index 00000000000..a8065d24e82 --- /dev/null +++ b/wrappers/python/applications/utils/src/opencv_realsense_camera.py @@ -0,0 +1,786 @@ + +''' +OpenCV like wrapper for Real Sense Camera + +================== + +Allows to read, display store video and images of RGB - Depth combinations in different formats. +Can extract left and right IR images. +Aligns RGB and Depth data. +Can save data as mp4 or single images. +Can control laser power, exposure and other parameters. + +Usage: + python opencv_realsense_camera.py + will run the camera and open the image window with live stream. + Use keys outlines in test() function to switch different modes + + Press 'd' to show different display options + Press 'e' to show exposure control + Press 'p' to show projector control + Press 'g' to show gain control + Press 's' to save the current image + Press 't' to save the left and right images in separate files + Press 'r' to start recording and one more time 'r' to stop video recording + Press 'a' to show advanced features + + +Environment : + ..\\Envs\\barcode + +Install : + pip install pyrealsense2-2.56.0.7981-cp310-cp310-win_amd64.whl + +''' +import os +import pyrealsense2 as rs +import numpy as np +import cv2 as cv +import time + +#%% Draw + +def draw_str(dst, target, s): + x, y = target + dst = cv.putText(dst, s, (x+1, y+1), cv.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 0), thickness = 2, lineType=cv.LINE_AA) + dst = cv.putText(dst, s, (x, y), cv.FONT_HERSHEY_PLAIN, 1.0, (255, 255, 255), lineType=cv.LINE_AA) + return dst + +#%% Helper +DS5_product_ids = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07", "0B3A", "0B5C", "0B5B"] + +def find_device_that_supports_advanced_mode() : + ctx = rs.context() + ds5_dev = rs.device() + devices = ctx.query_devices() + for dev in devices: + if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_product_ids: + if dev.supports(rs.camera_info.name): + print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name)) + return dev + raise Exception("No D400 product line device that supports advanced mode was found") + + +#%% Main +class RealSense(object): + def __init__(self, mode = 'rgb', use_ir = True, frame_size = None): + + self.frame_size = (1280, 720) if frame_size is None else frame_size #frame_size #(1280, 720)#(640,480) + self.display_mode = 'rgb' if mode is None else mode + self.use_ir = False if use_ir is None else use_ir + self.use_projector = False + self.use_advanced = False # advanced mode is enabled + self.control_mode = 'no controls' + self.DISPLAY_MODES = ['rgb','rgd','ddd','d16','gdd','scl','sc2','dep','iid','ii2','iig','iir','gd','ggd'] + + # noise measurement + self.img_int_mean = None + self.img_int_std = None + self.use_measure = False + self.rect = None + + # Configure depth and color streams + self.pipeline = rs.pipeline() + self.config = rs.config() + + # Get device product line for setting a supporting resolution + device_name = self.get_device_name() + self.set_frame_size(device_name) + + # start streaming + self.set_start_streaming() + + # set advanced mode - disparity in pixels + self.set_advanced_mode() + self.advance_mode = None + + # # turn emitter on-off + self.has_projector = device_name.find('D455') > 0 or device_name.find('D555') > 0 + self.switch_projector() + + # Depth controls to defaults + self.set_exposure() + self.set_gain() + self.set_laser_power() + + # Create an align object + # rs.align allows us to perform alignment of depth frames to others frames + # The "align_to" is the stream type to which we plan to align depth frames. + align_to = rs.stream.color + self.align = rs.align(align_to) + + # output support + self.output_range = [0,255] # extract range to map 16 bit to 8 + + # record video + self.vout = None + self.record_on = False # toggle recording + self.count = 0 + + def render(self, dst): + pass + + def get_device_name(self): + "find device name" + device_name = '' + pipeline_wrapper = rs.pipeline_wrapper(self.pipeline) + try: # 545 + pipeline_profile = self.config.resolve(pipeline_wrapper) + device = pipeline_profile.get_device() + device_product_line = str(device.get_info(rs.camera_info.product_line)) + device_name = device.get_info(rs.camera_info.name) + print('Device name : ', device_name) + print('Device product line : ', device_product_line) + except Exception as e: + print('Real Sense new version - possibly will require a new driver version') + print(e) + + return device_name + + def set_frame_size(self, device_name): + "device dependent data" + if device_name.find('D585') > 0 or device_name.find('D555') > 0: + print(f'Configured for {device_name}') + self.frame_size = (1280, 720) + + print(f'Frame size : {self.frame_size[0]} x {self.frame_size[1]}') + + def set_start_streaming(self): + "start stremaing" + self.config.enable_stream(rs.stream.depth, self.frame_size[0], self.frame_size[1], rs.format.z16, 30) + self.config.enable_stream(rs.stream.color, self.frame_size[0], self.frame_size[1], rs.format.bgr8, 30) + + if self.use_ir: + self.config.enable_stream(rs.stream.infrared, 1) + self.config.enable_stream(rs.stream.infrared, 2) + print('IR is enabled') + else: + print('IR is disabled') + + + # Start streaming + profile = self.pipeline.start(self.config) + + # Getting the depth sensor's depth scale (see rs-align example for explanation) + self.depth_sensor = profile.get_device().first_depth_sensor() + #depth_scale = self.depth_sensor.get_depth_scale() + #print("Depth Scale is: " , depth_scale) + + def set_advanced_mode(self): + "enable camera advanced mode" + if not self.use_advanced: + return + + try: + dev = find_device_that_supports_advanced_mode() + advnc_mode = rs.rs400_advanced_mode(dev) + print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") + + # Loop until we successfully enable advanced mode + while not advnc_mode.is_enabled(): + print("Trying to enable advanced mode...") + advnc_mode.toggle_advanced_mode(True) + # At this point the device will disconnect and re-connect. + print("Sleeping for 5 seconds...") + time.sleep(5) + # The 'dev' object will become invalid and we need to initialize it again + dev = find_device_that_supports_advanced_mode() + advnc_mode = rs.rs400_advanced_mode(dev) + print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") + + # Get each control's current value + print("Depth Control: \n", advnc_mode.get_depth_control()) + print("RSM: \n", advnc_mode.get_rsm()) + print("RAU Support Vector Control: \n", advnc_mode.get_rau_support_vector_control()) + print("Color Control: \n", advnc_mode.get_color_control()) + print("RAU Thresholds Control: \n", advnc_mode.get_rau_thresholds_control()) + print("SLO Color Thresholds Control: \n", advnc_mode.get_slo_color_thresholds_control()) + print("SLO Penalty Control: \n", advnc_mode.get_slo_penalty_control()) + print("HDAD: \n", advnc_mode.get_hdad()) + print("Color Correction: \n", advnc_mode.get_color_correction()) + print("Depth Table: \n", advnc_mode.get_depth_table()) + print("Auto Exposure Control: \n", advnc_mode.get_ae_control()) + print("Census: \n", advnc_mode.get_census()) + + except Exception as e: + print(e) + return + + #UD - enable disparity mode output + depth_table = advnc_mode.get_depth_table() + depth_table.disparityMode = 1 # 0-depth,1-disparity + advnc_mode.set_depth_table(depth_table) + print("Depth Table: \n", advnc_mode.get_depth_table()) # confirm the settings + + + # #UD - Simulator settings + hdad = advnc_mode.get_hdad() + hdad.ignoreSAD = 1 + advnc_mode.set_hdad(hdad) + print("HDAD: \n", advnc_mode.get_hdad()) + + color_cntrl = advnc_mode.get_color_control() + color_cntrl.disableSADColor = 1 + color_cntrl.disableRAUColor = 1 + advnc_mode.set_color_control(color_cntrl) + print("Color Correction: \n", advnc_mode.get_color_control()) + + # no difference + # rau_cntrl = advnc_mode.get_rau_support_vector_control() + # rau_cntrl.minWEsum = 1 + # rau_cntrl.minNSsum = 1 + # advnc_mode.set_rau_support_vector_control(rau_cntrl) + # print("RAU Support Vector Control: \n", advnc_mode.get_color_control()) + + rsm = advnc_mode.get_rsm() + rsm.rsmBypass = 1 + advnc_mode.set_rsm(rsm) + print("RSM: \n", advnc_mode.get_rsm()) + + depth_cntrl = advnc_mode.get_depth_control() + depth_cntrl.scoreThreshA = 0 + depth_cntrl.deepSeaSecondPeakThreshold = 50 + advnc_mode.set_depth_control(depth_cntrl) + print("Depth Control: \n", advnc_mode.get_depth_control()) + + slo_cntrl = advnc_mode.get_slo_penalty_control() + slo_cntrl.sloK1Penalty = 400 + slo_cntrl.sloK2Penalty = 511 + advnc_mode.set_slo_penalty_control(slo_cntrl) + print("SLO Penalty Control: \n", advnc_mode.get_slo_penalty_control()) + + #self.depth_sensor = dev + #return dev + + def set_exposure(self, exposure_value = None): + "set exposure to the correct values" + + if self.depth_sensor.supports(rs.option.exposure): + range = self.depth_sensor.get_option_range(rs.option.exposure) + exposure_value = exposure_value if exposure_value is not None else range.default + exposure_value = exposure_value if exposure_value > range.min else range.min + exposure_value = exposure_value if exposure_value < range.max else range.max + + self.depth_sensor.set_option(rs.option.exposure, int(exposure_value)) + print(f'Exposure is : {exposure_value}') + else: + print('Exposure has no support') + + def set_gain(self,gain_value = None): + "set gain to the correct values" + if self.depth_sensor.supports(rs.option.gain): + range = self.depth_sensor.get_option_range(rs.option.gain) + gain_value = gain_value if gain_value is not None else range.default + gain_value = gain_value if gain_value > range.min else range.min + gain_value = gain_value if gain_value < range.max else range.max + + self.depth_sensor.set_option(rs.option.gain, int(gain_value)) + print(f'Gain is : {gain_value}') + else: + print('Gain has no support') + + def set_output_range(self, range_value = 0): + "maps 16 bit to 8" + range_value = range_value * 255 + self.output_range[0] = range_value + self.output_range[1] = range_value + 255 + print(f'Output range is set to min {self.output_range[0]} and max {self.output_range[1]}') + + def get_baseline(self): + "returns camera baseline" + B = self.depth_sensor.get_option(rs.option.stereo_baseline) + print(f'Baseline is : {B} mm') + return B + + def get_focal_length(self): + "intrinsic parameters and returns focal length" + pipeline_wrapper = rs.pipeline_wrapper(self.pipeline) + pipeline_profile = self.config.resolve(pipeline_wrapper) + intr = pipeline_profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() + print(f'Intrinsics Fx is : {intr.fx} ') + return intr.fx + + def get_camera_intrinsics(self): + "intrinsic parameters of the camera" + pipeline_wrapper = rs.pipeline_wrapper(self.pipeline) + pipeline_profile = self.config.resolve(pipeline_wrapper) + intr = pipeline_profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() + #print(f'Intrinsics Fx is : {intr.fx} ') + print(intr) + return intr.fx + + def get_bf(self): + "read baseline and focal length for inverse depth compute" + b = self.get_baseline() + f = self.get_focal_length() + print(f'Total BF is : {b*f} ') + return b*f + + def get_camera_params(self, value_in = 0): + "whoch camera params toi show" + if value_in == 0: + self.get_bf() + elif value_in == 1: + self.get_camera_intrinsics() + + def set_laser_power(self, laser_power_value = None): + "set laser power to the correct values" + + if self.depth_sensor.supports(rs.option.laser_power): + range = self.depth_sensor.get_option_range(rs.option.laser_power) + laser_power_value = laser_power_value if laser_power_value is not None else range.default + laser_power_value = laser_power_value if laser_power_value > range.min else range.min + laser_power_value = laser_power_value if laser_power_value < range.max else range.max + + self.depth_sensor.set_option(rs.option.laser_power, int(laser_power_value)) + print(f'Laser power is : {laser_power_value}') + else: + print('Laser power has no support') + + def switch_projector(self): + "switch projectpr on-off" + if not self.has_projector: + print('Camera is without projector') + else: + #if self.use_projector is False: + self.depth_sensor.set_option(rs.option.emitter_always_on, self.use_projector) + self.depth_sensor.set_option(rs.option.emitter_enabled, self.use_projector) + + time.sleep(0.1) # wait for camera on - off + print('Camera projector : %s' %str(self.use_projector)) + + def switch_disparity(self): + "switch disparity on" + + if self.advance_mode is None: + dev = find_device_that_supports_advanced_mode() + advnc_mode = rs.rs400_advanced_mode(dev) + print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") + + # Loop until we successfully enable advanced mode + while not advnc_mode.is_enabled(): + print("Trying to enable advanced mode...") + advnc_mode.toggle_advanced_mode(True) + # At this point the device will disconnect and re-connect. + print("Sleeping for 5 seconds...") + time.sleep(5) + # The 'dev' object will become invalid and we need to initialize it again + dev = find_device_that_supports_advanced_mode() + advnc_mode = rs.rs400_advanced_mode(dev) + print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") + + self.advance_mode = advnc_mode + + depth_table = self.advance_mode.get_depth_table() + depth_table.disparityMode = 1 - depth_table.disparityMode # 0-depth,1-disparity - switch + self.advance_mode.set_depth_table(depth_table) + print("Depth Table: \n", self.advance_mode.get_depth_table()) # confirm the settings + + def set_display_mode(self, mode = 'rgb'): + "changes display mode by umber or by string" + + if isinstance(mode,int): # integer + mode = mode % len(self.DISPLAY_MODES) + mode = self.DISPLAY_MODES[mode] + + if not(mode in self.DISPLAY_MODES): + print(f'Not supported mode = {mode}') + + self.display_mode = mode + print(f'Current mode {mode}') + + def set_controls(self, value_in = 0): + "implements differnt controls according to the selected control mode. Input is an integer from 0-9" + if self.control_mode == 'display': + self.set_display_mode(value_in) + + elif self.control_mode == 'exposure': + self.set_exposure(value_in*2000) + + elif self.control_mode == 'gain': + self.set_gain(value_in*10) + + elif self.control_mode == 'projector': + self.use_projector = value_in == 1 + self.switch_projector() + + elif self.control_mode == 'disparity': + self.switch_disparity() + + elif self.control_mode == 'range': + self.set_output_range(value_in) + + elif self.control_mode == 'focal': + self.get_camera_params(value_in) + else: + pass + + def convert_depth_to_disparity(self, img_depth): + "from GIL" + focal_len = 175.910019 + baseline = 94.773 + #replacementDepth = focal_len * baseline / (RectScaledInfra1.x - (maxLoc.x + RectScaledInfra2.x)); + img_disparity = img_depth.copy() + valid = img_depth > 0 + img_disparity[valid]= focal_len*baseline/img_depth[valid]*32 + return img_disparity + + def measure_noise(self, img): + "makes integration over ROI" + x0, y0, x1, y1 = self.rect + if len(img.shape) < 3: + img_roi = img[y0:y1,x0:x1].astype(np.float32) + else: # protect from rgb display + img_roi = img[y0:y1,x0:x1,0].astype(np.float32) + + if self.img_int_mean is None: + self.img_int_mean = img_roi + self.img_int_std = np.zeros_like(img_roi) + elif self.img_int_mean.shape[1] != img_roi.shape[1]: # image display is changed + self.img_int_mean = None + return 0 + + valid_bool = img_roi > 0 + #valid_num = valid_bool.sum() + #nr,nc = img_roi.shape[:2] + + self.img_int_mean += 0.1*(img_roi - self.img_int_mean) + self.img_int_std += 0.1*(np.abs(img_roi - self.img_int_mean) - self.img_int_std) + + err_std_valid = self.img_int_std.copy() + #err_std_valid[~valid_bool] = 100 + err_std = err_std_valid[valid_bool].mean() + + return err_std + + def create_output_image(self, depth_image, color_image, irl_image, irr_image): + "defines the output image" + + if self.display_mode == 'rgb': + image_out = color_image + elif self.display_mode == 'ddd': + # Apply colormap on depth image (image must be converted to 8-bit per pixel first) + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.03) + depth_colormap = cv.applyColorMap(depth_scaled, cv.COLORMAP_JET) + image_out = depth_scaled + elif self.display_mode == 'rgd': + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.03) + image_out = np.concatenate((color_image[:,:,:2], depth_scaled[:,:,np.newaxis] ), axis = 2) + elif self.display_mode == 'gd': + gray_image = cv.cvtColor(color_image, cv.COLOR_RGB2GRAY) + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.03) + image_out = np.concatenate((gray_image, depth_scaled ), axis = 1) + elif self.display_mode == 'ggd': + gray_image = cv.cvtColor(color_image, cv.COLOR_RGB2GRAY) + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.03) + image_out = np.stack((gray_image, gray_image, depth_scaled ), axis = 2) + elif self.display_mode == 'gdd': + gray_image = cv.cvtColor(color_image, cv.COLOR_RGB2GRAY) + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.03) + image_out = np.stack((gray_image, depth_scaled, depth_scaled ), axis = 2) + elif self.display_mode == 'scl': + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.05) + image_out = cv.applyColorMap(depth_scaled, cv.COLORMAP_JET) + elif self.display_mode == 'sc2': + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.1) + image_out = cv.applyColorMap(depth_scaled, cv.COLORMAP_JET) + elif self.display_mode == 'ii2': + image_out = np.concatenate((irl_image, irr_image), axis = 1) + elif self.display_mode == 'iid': + #print(f'Depth {depth_image.min()} - {depth_image.max()}') + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.1) + image_out = np.stack((irl_image, irr_image, depth_scaled), axis = 2) + #image_out = np.concatenate((irl_image, depth_scaled), axis = 1) + elif self.display_mode == 'd16': + image_out = np.stack((irl_image.astype(np.uint16), irr_image.astype(np.uint16), depth_image), axis = 2) + elif self.display_mode == 'iig': + image_out = np.stack((irl_image, irr_image, color_image[:,:,1]), axis = 2) + elif self.display_mode == 'iir': + image_out = np.stack((irl_image, irr_image, color_image[:,:,0]), axis = 2) + #image_out = np.concatenate((irl_image, color_image[:,:,0]), axis = 1) + elif self.display_mode == 'dep': + image_out = depth_image.astype(np.float32) + image_out = image_out - self.output_range[0] + image_out[image_out < 0] = 0 + image_out[image_out > 255] = 255 + + + #image_out = depth_image / 32 * 4 # 10 for scaling + #image_out = self.convert_depth_to_disparity(depth_image) + return image_out + + def read(self, dst=None): + "with frame alignments and color space transformations" + #self.use_projector = not self.use_projector # testing + w, h = self.frame_size + + # Wait for a coherent pair of frames: depth and color + frames = self.pipeline.wait_for_frames() + # Align the depth frame to color frame + aligned_frames = self.align.process(frames) + + # Get aligned frames + depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image + color_frame = aligned_frames.get_color_frame() + if not depth_frame or not color_frame: + return False, None + + # Convert images to numpy arrays + depth_image = np.asanyarray(depth_frame.get_data()) + color_image = np.asanyarray(color_frame.get_data()) + #color_image = cv.cvtColor(depth_image, cv.COLOR_GRAY2RGB) + #depth_image = cv.cvtColor(color_image, cv.COLOR_RGB2GRAY) + + # Apply colormap on depth image (image must be converted to 8-bit per pixel first) + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.03) + depth_colormap = cv.applyColorMap(depth_scaled, cv.COLORMAP_JET) + + depth_colormap_dim = depth_colormap.shape + color_colormap_dim = color_image.shape + + #If depth and color resolutions are different, resize color image to match depth image for display + if depth_colormap_dim != color_colormap_dim: + raise ValueError('depth and image size missmatch') + #color_image = cv.resize(color_image, dsize=(depth_colormap_dim[1], depth_colormap_dim[0]), interpolation=cv.INTER_AREA) + #images = np.hstack((resized_color_image, depth_colormap)) + # else: + # images = np.hstack((color_image, depth_colormap)) + + if self.use_ir: + ir_left = aligned_frames.get_infrared_frame(1) + irl_image = np.asanyarray(ir_left.get_data()) + ir_right = aligned_frames.get_infrared_frame(2) + irr_image = np.asanyarray(ir_right.get_data()) + else: + #print('Enable IR use at the start. use_ir = True') + irl_image = color_image[:,:,0] + irr_image = color_image[:,:,1] + image_out = color_image + + image_out = self.create_output_image(depth_image, color_image, irl_image, irr_image) + return True, image_out + + def read_not_aligned(self, dst=None): + "color and depth are not aligned" + w, h = self.frame_size + + # Wait for a coherent pair of frames: depth and color + frames = self.pipeline.wait_for_frames() + depth_frame = frames.get_depth_frame() + color_frame = frames.get_color_frame() + if not depth_frame or not color_frame: + return False, None + + # Convert images to numpy arrays + depth_image = np.asanyarray(depth_frame.get_data()) + color_image = np.asanyarray(color_frame.get_data()) + #color_image = cv.cvtColor(depth_image, cv.COLOR_GRAY2RGB) + #depth_image = cv.cvtColor(color_image, cv.COLOR_RGB2GRAY) + + # Apply colormap on depth image (image must be converted to 8-bit per pixel first) + depth_scaled = cv.convertScaleAbs(depth_image, alpha=0.03) + depth_colormap = cv.applyColorMap(depth_scaled, cv.COLORMAP_JET) + + depth_colormap_dim = depth_colormap.shape + color_colormap_dim = color_image.shape + + image_out = depth_image + return True, image_out + + def isOpened(self): + "OpenCV compatability" + return True + + def save_image(self, frame): + fn = '.\\image_%s_%03d.png' % (self.display_mode, self.count) + #frame = cv.cvtColor(frame, cv.CV_16U) + cv.imwrite(fn, frame) + print(fn, 'saved') + self.count += 1 + + def save_two_images(self, frame): + "saves two differnet files" + if len(frame.shape) < 3: + print('Image should have 3 chnnels. Try differnet display options') + return + + fl = '.\\imageL_%s_%03d.png' % (self.display_mode, self.count) + cv.imwrite(fl, frame[:,:,0]) + fr = '.\\imageR_%s_%03d.png' % (self.display_mode, self.count) + cv.imwrite(fr, frame[:,:,1]) + print('Saving %s and %s' %(fl,fr)) + self.count += 1 + + def record_video(self, frame): + # record video to a file is switched on + if (self.vout is None) and (self.record_on is True): + fourcc = cv.VideoWriter_fourcc(*'mp4v') + k = 0 + fname = '.\\video_%s_%03d.mp4' % (self.display_mode,k) + while os.path.exists(fname): + k +=1 + fname = '.\\video_%s_%03d.mp4' % (self.display_mode,k) + + self.vout = cv.VideoWriter(fname, fourcc, 20.0, self.frame_size) + print('Writing video to file %s' %fname) + self.count = 0 + + # write frame + if (self.vout is not None) and (self.record_on is True): + "" + if len(frame.shape) < 3: + frame = frame[:self.frame_size[1],:self.frame_size[0]] + frame = cv.cvtColor(frame, cv.COLOR_GRAY2RGB) + + self.vout.write(frame) + self.count += 1 + if self.count % 100 == 0: + print('Writing frame %s' %str(self.count)) + + # record on is switched off + if (self.vout is not None) and (self.record_on is False): + self.vout.release() + self.vout = None + print('Video file created') + + def record_release(self): + "finish record" + if self.vout is not None: + self.vout.release() + self.vout = None + print('Video file created') + + def show_controls(self, frame): + "show image on opencv window" + if self.control_mode == 'display': + frame = cv.putText(frame, 'Display (0-RGB, 1,2,3...9-I1+I2)', (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (200,200,12), 2) + + elif self.control_mode == 'exposure': + frame = cv.putText(frame, 'Exposure (1,2,3...9) ', (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (200,200,200), 2) + + elif self.control_mode == 'gain': + frame = cv.putText(frame, 'Gain (1,2,3...9) ', (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (200,200,200), 2) + + elif self.control_mode == 'projector': + frame = cv.putText(frame, 'Projector (0,1) ', (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (200,200,200), 2) + + elif self.control_mode == 'disparity': + frame = cv.putText(frame, 'Disparity Out (0,1) ', (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (200,200,200), 2) + + elif self.control_mode == 'range': + frame = cv.putText(frame, 'Bit Range Out (0,1,2...9) ', (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (200,200,200), 2) + + elif self.control_mode == 'focal': + frame = cv.putText(frame, 'Camera Params (0-BL+F,1-Cam Mtrx+Dist, 2..) ', (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (200,200,200), 2) + else: + pass + + return frame + + def show_measurements(self,frame): + "show measurements of the noise" + if not self.use_measure: + self.img_int_mean = None # reset when enabled + return frame + + if self.rect is None: + h,w = frame.shape[0]>>1, frame.shape[1]>>1 + h2,w2 = h>>2,w>>2 + self.rect = [w-h2, h-h2, w+h2, h+h2] + + err_std = self.measure_noise(frame) + # show min and max + print(f'Frame min {frame.min()} and max {frame.max()}') + + x0, y0, x1, y1 = self.rect + clr = (0, 0, 0) if frame[y0:y1,x0:x1].mean() > 128 else (240,240,240) + frame = cv.rectangle(frame, (x0, y0), (x1, y1), clr, 2) + frame = draw_str(frame,(x0,y0-10),str(err_std)) + + return frame + + def show_image(self, frame): + "show image on opencv window" + do_exit = False + frame_show = np.uint8(frame.copy()) + + frame_show = self.show_controls(frame_show) + + cv.imshow('frame (d,e,g,f,p,o,g,m,s,t,r: q - to exit)', frame_show) + ch = cv.waitKeyEx(1) & 0xff + if ch == ord('q') or ch == 27: + do_exit = True + elif ch in np.arange(48,58) : # numbers only + self.set_controls(ch - 48) + elif ch in np.arange(65,75) : # 2 digit numbers using SHIFT key and keys a,b,c,d,e,f,g + self.set_controls(ch - 55) + elif ch == ord('d'): # depth image + self.control_mode = 'no controls' if self.control_mode == 'display' else 'display' + elif ch == ord('e'): # exposure control + self.control_mode = 'no controls' if self.control_mode == 'exposure' else 'exposure' + elif ch == ord('g'): # exposure control + self.control_mode = 'no controls' if self.control_mode == 'gain' else 'gain' + elif ch == ord('p'): + self.control_mode = 'no controls' if self.control_mode == 'projector' else 'projector' + elif ch == ord('o'): + self.control_mode = 'no controls' if self.control_mode == 'range' else 'range' + elif ch == ord('x'): + self.control_mode = 'no controls' if self.control_mode == 'disparity' else 'disparity' + elif ch == ord('f'): + self.control_mode = 'no controls' if self.control_mode == 'focal' else 'focal' + elif ch == ord('m'): + self.use_measure = not self.use_measure + print(f'Noise measurement is {self.use_measure}') + elif ch == ord('s'): + self.save_image(frame) + elif ch == ord('t'): + self.save_two_images(frame) + elif ch == 2490368: # Left: 2424832 Up: 2490368 Right: 2555904 Down: 2621440 + pass + elif ch == ord('a'): # enable advanced mode + self.use_advanced = not self.use_advanced + self.set_advanced_mode() + elif ch == ord('r'): + self.record_on = not self.record_on + print('Video record %s' %str(self.record_on)) + elif ch != 255: + print(f'Unrecognized key {ch} - check your language setttings on the keyboard, must be English.') + return do_exit + + def close(self): + # stop record + self.record_release() + + # Stop streaming + self.pipeline.stop() + #self.depth_sensor.stop() + #self.depth_sensor.close() + print('closed') + + def release(self): + "opencv compatability" + self.close() + + def test(self): + while True: + ret, frame = self.read() + if ret is False: + break + + frame = self.show_measurements(frame) + ret = self.show_image(frame) + if ret : + break + + # check if record is required + self.record_video(frame) + + if ret is False: + print('Failed to read image') + else: + self.close() + cv.destroyAllWindows() + +if __name__ == '__main__': + cap = RealSense() #frame_size=(640,360)) + cap.test() \ No newline at end of file diff --git a/wrappers/python/applications/utils/test/test_camera_noise_estimator.py b/wrappers/python/applications/utils/test/test_camera_noise_estimator.py new file mode 100644 index 00000000000..4a651535151 --- /dev/null +++ b/wrappers/python/applications/utils/test/test_camera_noise_estimator.py @@ -0,0 +1,352 @@ + +''' +Noise Measurement App - Tester +================== + +Using depth image to measure noise in a specific selected ROI. + +Usage +----- + Run NoiseApp - it will connect to the RealSense camera. + Select different measurement options: + - Press ' + Manually select ROI + + +Environemt : +----- + .\\Envs\\barcode + +Install : + + +''' +import sys +import numpy as np +import cv2 as cv +import matplotlib.pyplot as plt + +# importing common Use modules +sys.path.append(r'wrappers\python\applications\utils\src') +from opencv_realsense_camera import RealSense +from camera_noise_estimator import NoiseEstimator +from logger import log + +#%% Helpers +def draw_axis(img, rvec, tvec, cam_mtrx, cam_dist, len = 10): + # unit is mm + points = np.float32([[len, 0, 0], [0, len, 0], [0, 0, len], [0, 0, 0]]).reshape(-1, 3) + axisPoints, _ = cv.projectPoints(points, rvec, tvec, cam_mtrx, cam_dist) + axisPoints = axisPoints.squeeze().astype(np.int32) + img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0,0,255), 3) + img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0,255,0), 3) + img = cv.line(img, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255,0,0), 3) + return img + +def draw_str(dst, target, s): + x, y = target + dst = cv.putText(dst, s, (x+1, y+1), cv.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 0), thickness = 2, lineType=cv.LINE_AA) + dst = cv.putText(dst, s, (x, y), cv.FONT_HERSHEY_PLAIN, 1.0, (255, 255, 255), lineType=cv.LINE_AA) + return dst + + +#%% ROI selector from OpenCV +class RectSelector: + def __init__(self, win, callback): + self.win = win + self.callback = callback + cv.setMouseCallback(win, self.onmouse) + self.drag_start = None + self.drag_rect = None + def onmouse(self, event, x, y, flags, param): + x, y = np.int16([x, y]) # BUG + if event == cv.EVENT_LBUTTONDOWN: + self.drag_start = (x, y) + return + if self.drag_start: + if flags & cv.EVENT_FLAG_LBUTTON: + xo, yo = self.drag_start + x0, y0 = np.minimum([xo, yo], [x, y]) + x1, y1 = np.maximum([xo, yo], [x, y]) + self.drag_rect = None + if x1-x0 > 0 and y1-y0 > 0: + self.drag_rect = (x0, y0, x1, y1) + else: + rect = self.drag_rect + self.drag_start = None + self.drag_rect = None + if rect: + self.callback(rect) + def draw(self, vis): + if not self.drag_rect: + return False + x0, y0, x1, y1 = self.drag_rect + cv.rectangle(vis, (x0, y0), (x1, y1), (0, 255, 0), 2) + return True + @property + def dragging(self): + return self.drag_rect is not None + +#%% Help function to display results +class DataDisplay: + def __init__(self): + self.tracker = None + + def convert_plane_params(self, plane_equation): + "convert plane params to rvec" + # 4. Convert plane parameters to rvec and tvec + # - The plane normal vector is (A, B, C). + # - We can use the normal vector to get the rotation. + # - A point on the plane can be used for the translation vector. + + # Normalize the plane normal vector + normal = plane_equation #np.array([plane_equation[0], plane_equation[1], plane_equation[2]]) + normal_norm = np.linalg.norm(normal) + if normal_norm == 0: + print("Error: Zero norm for plane normal vector.") + return None + normal = normal / normal_norm + + # Use the normalized normal vector to get the rotation matrix + # This is a common method, but there are other ways to do this. + z_axis = np.array([0, 0, 1]) + rotation_axis = np.cross(z_axis, normal) + rotation_angle = np.arccos(np.dot(z_axis, normal)) + + # Handle the case where the rotation axis is zero (normal is parallel to z-axis) + if np.linalg.norm(rotation_axis) < 1e-6: + if normal[2] > 0: + rvec = np.zeros(3) # Rotation is identity + else: + rvec = np.array([0, np.pi, 0]) # Rotation by 180 degrees around X or Y. + else: + rvec, _ = cv.Rodrigues(rotation_axis * rotation_angle) + + return rvec + + def show_axis(self, vis): + "draw axis after plane estimation" + if self.tracker.plane_params is None: + return vis + + #rvec = self.plane_params/np.sum(self.plane_params**2) # normalize + rvec = self.convert_plane_params(self.tracker.plane_params) + #rvec = self.plane_to_rvec(self.plane_params) + + tvec = self.tracker.plane_center + vis = draw_axis(vis, rvec, tvec, self.tracker.cam_matrix, self.tracker.cam_distort, len = 10) + return vis + + def show_rect_and_text(self, vis): + "draw axis after plane estimation" + err_mean, err_std = self.tracker.img_mean, self.tracker.img_std + if err_mean is None: + return vis + + + measure_type = self.tracker.measure_type + img_fill = self.tracker.img_fill + x0, y0, x1, y1 = self.tracker.rect + + clr = (0, 0, 0) if vis[y0:y1,x0:x1].mean() > 128 else (240,240,240) + vis = cv.rectangle(vis, (x0, y0), (x1, y1), clr, 2) + txt = f'{measure_type}:{err_mean:.2f}-{err_std:.3f}' + if measure_type == 'F': + txt = f'{measure_type}:{img_fill:.2f} %' + vis = draw_str(vis,(x0,y0-10),txt) + + return vis + + def show_scene(self, tracker, vis): + "draw ROI and Info" + self.tracker = tracker + vis = self.show_rect_and_text(vis) + vis = self.show_axis(vis) + + return vis + + def show_measurements(self, depth_true = None): + "show roi statistics for all frames" + image_num = len(self.depth_mean) + if image_num < 2: + #log.info('No stat data is collected') + return + + d_mean = np.array(self.depth_mean) + d_std = np.array(self.depth_std) + t_frame = np.arange(image_num) + + # true data is set + x_axis_is_depth = False + if depth_true is not None: + t_frame[:len(depth_true)] = np.array(depth_true) + x_axis_is_depth = True + #print(d_mean) + #print(t_frame) + d_mean = d_mean - t_frame + + + # fig, ax = plt.subplots() + # fig.set_size_inches([24, 16]) + # ax.set_title('Depth Noise vs Distance') + # ax.set_xlabel('Frame [#]') + # ax.set_ylabel('Distance [mm]') + # ax.scatter(t_frame, d_mean, c='b', lw=3, label='mean') + # ax.scatter(t_frame, d_mean+d_std, c='r', lw=1, label='mean+std') + # ax.scatter(t_frame, d_mean-d_std, c='g', lw=1, label='mean-std') + # ax.legend() + # plt.show() + + fig, ax = plt.subplots() + fig.set_size_inches([24, 16]) + ax.set_title('Depth Noise vs Distance') + if x_axis_is_depth: + ax.set_xlabel('Distance [mm]') + else: + ax.set_xlabel('Frame [#]') + + ax.set_ylabel('Distance Error [mm]') + ax.scatter(t_frame, d_mean, c='r', lw=3, label='mean err') + ax.scatter(t_frame, d_mean-d_std, c='b', lw=3, label='mean err-std') + ax.scatter(t_frame, d_mean+d_std, c='g', lw=3, label='mean err+std') + ax.legend() + + + if x_axis_is_depth: + fig, ax = plt.subplots() + fig.set_size_inches([24, 16]) + ax.set_title('Depth Noise vs Distance') + ax.set_xlabel('Distance [mm]') + ax.set_ylabel('Distance Error [mm]') + ax.plot(t_frame, d_mean, '-o', label='mean') + ax.legend() + + + plt.show() + + return + + +#%% App that uses camera and Noise estimator +class NoiseApp: + def __init__(self): + self.cap = RealSense() # + self.cap.set_display_mode('iid') + self.frame = None + self.rect = None + self.paused = False + self.trackers = [] + self.display = DataDisplay() + + self.show_dict = {} # hist show + self.measure_type = 'T' + self.win_name = 'Noise (q-quit, a,f,p,s,t)' + + cv.namedWindow(self.win_name ) + self.rect_sel = RectSelector(self.win_name , self.on_rect) + + def define_roi(self, image): + '''define ROI target.''' + h,w = image.shape[:2] + rect = [0, 0, w, h] + return rect + + def on_rect(self, rect): + "remember ROI defined by user" + #self.define_roi(self.frame, rect) + tracker = NoiseEstimator() #estimator_type=self.estim_type, estimator_id=estim_ind) + tracker.rect = rect + tracker.measure_type = self.measure_type + self.trackers.append(tracker) + log.info(f'Adding noise estimator at : {rect}') + + def process_image(self, img_depth): + "makes measurements" + for tracker in self.trackers: + tracker.do_measurements(img_depth) + + def show_scene(self, frame): + "draw ROI and Info" + vis = np.uint8(frame.copy()) + vis = cv.cvtColor(vis, cv.COLOR_GRAY2BGR) + #vis = cv.convertScaleAbs(self.frame, alpha=0.03) + self.rect_sel.draw(vis) + + for tracker in self.trackers: + vis = self.display.show_scene(tracker, vis) + + return vis + + def show_histogram(self, img): + "show roi histgram" + if self.rect is None: + #print('define ROI') + return 0 + + x0, y0, x1, y1 = self.rect + img_roi = img[y0:y1,x0:x1].astype(np.float32) + # Compute histogram + hist, bins = np.histogram(img_roi.flatten(), bins=1024, range=[0, 2**15]) + + if not 'fig' in self.show_dict : #len(self.show_dict) < 1: + fig, ax = plt.subplots() + fig.set_size_inches([24, 16]) + ax.set_title('Histogram (Depth)') + ax.set_xlabel('Bin') + ax.set_ylabel('Frequency') + lineGray, = ax.plot(bins[:-1], hist, c='k', lw=3) + ax.set_xlim(bins[0], bins[-1]) + ax.set_ylim(0, max(hist)+10) + plt.ion() + #plt.show() + + self.show_dict = {'fig':fig, 'ax':ax, 'line':lineGray} + else: + self.show_dict['line'].set_ydata(hist) + + self.show_dict['fig'].canvas.draw() + return + + def run(self): + while True: + playing = not self.paused and not self.rect_sel.dragging + if playing or self.frame is None: + ret, frame = self.cap.read() + if not ret: + break + self.frame = frame.copy() + + img_depth = self.frame[:,:,2] + + + #self.statistics(frame) + self.process_image(img_depth) + + #self.show_histogram(frame) + + vis = self.show_scene(img_depth) + + + cv.imshow(self.win_name , vis) + ch = cv.waitKey(1) + if ch == ord(' '): + self.paused = not self.paused + elif ch == ord('a'): + self.measure_type = 'A' + elif ch == ord('f'): + self.measure_type = 'F' + elif ch == ord('p'): + self.measure_type = 'P' + elif ch == ord('s'): + self.measure_type = 'S' + elif ch == ord('t'): + self.measure_type = 'T' + if ch == ord('c'): + if len(self.trackers) > 0: + t = self.trackers.pop() + if ch == 27 or ch == ord('q'): + break + +if __name__ == '__main__': + #print(__doc__) + NoiseApp().run() \ No newline at end of file