Merge branch 4.x

2026-07-28 23:03:03 +04:00 · 2023-06-01 09:37:38 +03:00
parent d24ffe9a65 fc5d412ba7
commit 5af40a0269
565 changed files with 84396 additions and 17589 deletions
@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+
+"""aruco_detect_board_charuco.py
+Usage example:
+python aruco_detect_board_charuco.py -w=5 -h=7 -sl=0.04 -ml=0.02 -d=10 -c=../data/aruco/tutorial_camera_charuco.yml
+                                     -i=../data/aruco/choriginal.jpg
+"""
+
+import argparse
+import numpy as np
+import cv2 as cv
+import sys
+
+
+def read_camera_parameters(filename):
+    fs = cv.FileStorage(cv.samples.findFile(filename, False), cv.FileStorage_READ)
+    if fs.isOpened():
+        cam_matrix = fs.getNode("camera_matrix").mat()
+        dist_coefficients = fs.getNode("distortion_coefficients").mat()
+        return True, cam_matrix, dist_coefficients
+    return False, [], []
+
+
+def main():
+    # parse command line options
+    parser = argparse.ArgumentParser(description="detect markers and corners of charuco board, estimate pose of charuco"
+                                     "board", add_help=False)
+    parser.add_argument("-H", "--help", help="show help", action="store_true", dest="show_help")
+    parser.add_argument("-v", "--video", help="Input from video or image file, if omitted, input comes from camera",
+                        default="", action="store", dest="v")
+    parser.add_argument("-i", "--image", help="Input from image file", default="", action="store", dest="img_path")
+    parser.add_argument("-w", help="Number of squares in X direction", default="3", action="store", dest="w", type=int)
+    parser.add_argument("-h", help="Number of squares in Y direction", default="3", action="store", dest="h", type=int)
+    parser.add_argument("-sl", help="Square side length", default="1.", action="store", dest="sl", type=float)
+    parser.add_argument("-ml", help="Marker side length", default="0.5", action="store", dest="ml", type=float)
+    parser.add_argument("-d", help="dictionary: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2,  DICT_4X4_1000=3,"
+                                   "DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, DICT_6X6_50=8,"
+                                   "DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12, DICT_7X7_100=13,"
+                                   "DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}",
+                        default="0", action="store", dest="d", type=int)
+    parser.add_argument("-ci", help="Camera id if input doesnt come from video (-v)", default="0", action="store",
+                        dest="ci", type=int)
+    parser.add_argument("-c", help="Input file with calibrated camera parameters", default="", action="store",
+                        dest="cam_param")
+
+    args = parser.parse_args()
+
+    show_help = args.show_help
+    if show_help:
+        parser.print_help()
+        sys.exit()
+    width = args.w
+    height = args.h
+    square_len = args.sl
+    marker_len = args.ml
+    dict = args.d
+    video = args.v
+    camera_id = args.ci
+    img_path = args.img_path
+
+    cam_param = args.cam_param
+    cam_matrix = []
+    dist_coefficients = []
+    if cam_param != "":
+        _, cam_matrix, dist_coefficients = read_camera_parameters(cam_param)
+
+    aruco_dict = cv.aruco.getPredefinedDictionary(dict)
+    board_size = (width, height)
+    board = cv.aruco.CharucoBoard(board_size, square_len, marker_len, aruco_dict)
+    charuco_detector = cv.aruco.CharucoDetector(board)
+
+    image = None
+    input_video = None
+    wait_time = 10
+    if video != "":
+        input_video = cv.VideoCapture(cv.samples.findFileOrKeep(video, False))
+        image = input_video.retrieve()[1] if input_video.grab() else None
+    elif img_path == "":
+        input_video = cv.VideoCapture(camera_id)
+        image = input_video.retrieve()[1] if input_video.grab() else None
+    elif img_path != "":
+        wait_time = 0
+        image = cv.imread(cv.samples.findFile(img_path, False))
+
+    if image is None:
+        print("Error: unable to open video/image source")
+        sys.exit(0)
+
+    while image is not None:
+        image_copy = np.copy(image)
+        charuco_corners, charuco_ids, marker_corners, marker_ids = charuco_detector.detectBoard(image)
+        if not (marker_ids is None) and len(marker_ids) > 0:
+            cv.aruco.drawDetectedMarkers(image_copy, marker_corners)
+        if not (charuco_ids is None) and len(charuco_ids) > 0:
+            cv.aruco.drawDetectedCornersCharuco(image_copy, charuco_corners, charuco_ids)
+            if len(cam_matrix) > 0 and len(charuco_ids) >= 4:
+                try:
+                    obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
+                    flag, rvec, tvec = cv.solvePnP(obj_points, img_points, cam_matrix, dist_coefficients)
+                    if flag:
+                        cv.drawFrameAxes(image_copy, cam_matrix, dist_coefficients, rvec, tvec, .2)
+                except cv.error as error_inst:
+                    print("SolvePnP recognize calibration pattern as non-planar pattern. To process this need to use "
+                          "minimum 6 points. The planar pattern may be mistaken for non-planar if the pattern is "
+                          "deformed or incorrect camera parameters are used.")
+                    print(error_inst.err)
+        cv.imshow("out", image_copy)
+        key = cv.waitKey(wait_time)
+        if key == 27:
+            break
+        image = input_video.retrieve()[1] if input_video is not None and input_video.grab() else None
+
+
+if __name__ == "__main__":
+    main()
@@ -5,11 +5,21 @@ camera calibration for distorted images with chess board samples
 reads distorted images, calculates the calibration and write undistorted images

 usage:
-    calibrate.py [--debug <output path>] [--square_size] [<image mask>]
+    calibrate.py [--debug <output path>] [-w <width>] [-h <height>] [-t <pattern type>] [--square_size=<square size>]
+    [--marker_size=<aruco marker size>] [--aruco_dict=<aruco dictionary name>] [<image mask>]
+
+usage example:
+    calibrate.py -w 4 -h 6 -t chessboard --square_size=50 ../data/left*.jpg

 default values:
    --debug:    ./output/
-    --square_size: 1.0
+    -w: 4
+    -h: 6
+    -t: chessboard
+    --square_size: 50
+    --marker_size: 25
+    --aruco_dict: DICT_4X4_50
+    --threads: 4
    <image mask> defaults to ../data/left*.jpg
 '''

@@ -30,31 +40,81 @@ def main():
    import getopt
    from glob import glob

-    args, img_mask = getopt.getopt(sys.argv[1:], '', ['debug=', 'square_size=', 'threads='])
+    args, img_names = getopt.getopt(sys.argv[1:], 'w:h:t:', ['debug=','square_size=', 'marker_size=',
+                                                      'aruco_dict=', 'threads=', ])
    args = dict(args)
    args.setdefault('--debug', './output/')
-    args.setdefault('--square_size', 1.0)
+    args.setdefault('-w', 4)
+    args.setdefault('-h', 6)
+    args.setdefault('-t', 'chessboard')
+    args.setdefault('--square_size', 10)
+    args.setdefault('--marker_size', 5)
+    args.setdefault('--aruco_dict', 'DICT_4X4_50')
    args.setdefault('--threads', 4)
-    if not img_mask:
-        img_mask = '../data/left??.jpg'  # default
-    else:
-        img_mask = img_mask[0]

-    img_names = glob(img_mask)
+    if not img_names:
+        img_mask = '../data/left??.jpg'  # default
+        img_names = glob(img_mask)
+
    debug_dir = args.get('--debug')
    if debug_dir and not os.path.isdir(debug_dir):
        os.mkdir(debug_dir)
-    square_size = float(args.get('--square_size'))

-    pattern_size = (9, 6)
-    pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
-    pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
-    pattern_points *= square_size
+    height = int(args.get('-h'))
+    width = int(args.get('-w'))
+    pattern_type = str(args.get('-t'))
+    square_size = float(args.get('--square_size'))
+    marker_size = float(args.get('--marker_size'))
+    aruco_dict_name = str(args.get('--aruco_dict'))
+
+    pattern_size = (height, width)
+    if pattern_type == 'chessboard':
+        pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
+        pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
+        pattern_points *= square_size
+    elif pattern_type == 'charucoboard':
+        pattern_points = np.zeros((np.prod((height-1, width-1)), 3), np.float32)
+        pattern_points[:, :2] = np.indices((height-1, width-1)).T.reshape(-1, 2)
+        pattern_points *= square_size
+    else:
+        print("unknown pattern")
+        return None

    obj_points = []
    img_points = []
    h, w = cv.imread(img_names[0], cv.IMREAD_GRAYSCALE).shape[:2]  # TODO: use imquery call to retrieve results

+    aruco_dicts = {
+        'DICT_4X4_50':cv.aruco.DICT_4X4_50,
+        'DICT_4X4_100':cv.aruco.DICT_4X4_100,
+        'DICT_4X4_250':cv.aruco.DICT_4X4_250,
+        'DICT_4X4_1000':cv.aruco.DICT_4X4_1000,
+        'DICT_5X5_50':cv.aruco.DICT_5X5_50,
+        'DICT_5X5_100':cv.aruco.DICT_5X5_100,
+        'DICT_5X5_250':cv.aruco.DICT_5X5_250,
+        'DICT_5X5_1000':cv.aruco.DICT_5X5_1000,
+        'DICT_6X6_50':cv.aruco.DICT_6X6_50,
+        'DICT_6X6_100':cv.aruco.DICT_6X6_100,
+        'DICT_6X6_250':cv.aruco.DICT_6X6_250,
+        'DICT_6X6_1000':cv.aruco.DICT_6X6_1000,
+        'DICT_7X7_50':cv.aruco.DICT_7X7_50,
+        'DICT_7X7_100':cv.aruco.DICT_7X7_100,
+        'DICT_7X7_250':cv.aruco.DICT_7X7_250,
+        'DICT_7X7_1000':cv.aruco.DICT_7X7_1000,
+        'DICT_ARUCO_ORIGINAL':cv.aruco.DICT_ARUCO_ORIGINAL,
+        'DICT_APRILTAG_16h5':cv.aruco.DICT_APRILTAG_16h5,
+        'DICT_APRILTAG_25h9':cv.aruco.DICT_APRILTAG_25h9,
+        'DICT_APRILTAG_36h10':cv.aruco.DICT_APRILTAG_36h10,
+        'DICT_APRILTAG_36h11':cv.aruco.DICT_APRILTAG_36h11
+    }
+
+    if (aruco_dict_name not in set(aruco_dicts.keys())):
+        print("unknown aruco dictionary name")
+        return None
+    aruco_dict = cv.aruco.getPredefinedDictionary(aruco_dicts[aruco_dict_name])
+    board = cv.aruco.CharucoBoard(pattern_size, square_size, marker_size, aruco_dict)
+    charuco_detector = cv.aruco.CharucoDetector(board)
+
    def processImage(fn):
        print('processing %s... ' % fn)
        img = cv.imread(fn, cv.IMREAD_GRAYSCALE)
@@ -63,10 +123,20 @@ def main():
            return None

        assert w == img.shape[1] and h == img.shape[0], ("size: %d x %d ... " % (img.shape[1], img.shape[0]))
-        found, corners = cv.findChessboardCorners(img, pattern_size)
-        if found:
-            term = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_COUNT, 30, 0.1)
-            cv.cornerSubPix(img, corners, (5, 5), (-1, -1), term)
+        found = False
+        corners = 0
+        if pattern_type == 'chessboard':
+            found, corners = cv.findChessboardCorners(img, pattern_size)
+            if found:
+                term = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_COUNT, 30, 0.1)
+                cv.cornerSubPix(img, corners, (5, 5), (-1, -1), term)
+        elif pattern_type == 'charucoboard':
+            corners, _charucoIds, _markerCorners_svg, _markerIds_svg = charuco_detector.detectBoard(img)
+            if (len(corners) == (height-1)*(width-1)):
+                found = True
+        else:
+            print("unknown pattern type", pattern_type)
+            return None

        if debug_dir:
            vis = cv.cvtColor(img, cv.COLOR_GRAY2BGR)
@@ -76,7 +146,7 @@ def main():
            cv.imwrite(outfile, vis)

        if not found:
-            print('chessboard not found')
+            print('pattern not found')
            return None

        print('           %s... OK' % fn)
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+
+import numpy as np
+import sys
+import cv2 as cv
+
+
+def main():
+    # Open Orbbec depth sensor
+    orbbec_cap = cv.VideoCapture(0, cv.CAP_OBSENSOR)
+    if orbbec_cap.isOpened() == False:
+        sys.exit("Fail to open camera.")
+
+    while True:
+        # Grab data from the camera
+        if orbbec_cap.grab():
+            # RGB data
+            ret_bgr, bgr_image = orbbec_cap.retrieve(None, cv.CAP_OBSENSOR_BGR_IMAGE)
+            if ret_bgr:
+                cv.imshow("BGR", bgr_image)
+
+            # depth data
+            ret_depth, depth_map = orbbec_cap.retrieve(None, cv.CAP_OBSENSOR_DEPTH_MAP)
+            if ret_depth:
+                color_depth_map = cv.normalize(depth_map, None, 0, 255, cv.NORM_MINMAX, cv.CV_8UC1)
+                color_depth_map = cv.applyColorMap(color_depth_map, cv.COLORMAP_JET)
+                cv.imshow("DEPTH", color_depth_map)
+        else:
+            print("Fail to grab data from the camera.")
+
+        if cv.pollKey() >= 0:
+            break
+
+    orbbec_cap.release()
+
+
+if __name__ == '__main__':
+    main()