3D pose estimation using aruco tag in python

Reprinted from: https://www.it610.com/article/1291934151255072768.htm

3D pose estimation using aruco tag in python

ArUco tag

First, what is the aruco tag?

ArUco marker is a binary square reference marker that can be used for camera attitude estimation. Its main advantage is that the detection is simple, fast and robust. ArUco mark is a square mark composed of a wide black border and an internal binary matrix that determines its identifier (id). The black border of ArUco mark is helpful for its rapid detection in the image, and the internal binary coding is used to identify the mark and provide error detection and correction. The size of the ArUco tag determines the size of the internal matrix. For example, a tag with a size of 4x4 is composed of 16 bit binary numbers.

Generally speaking, aruco tag is actually a kind of coding, which is similar to the QR code in our daily life. However, due to different coding methods, the way and capacity of storing information are different, so it will be different at the application level. Because a single aruco marker can provide sufficient correspondence, such as four obvious corners and internal binary coding, aruco markers are widely used to increase the amount of information when mapping from the two-dimensional world to the three-dimensional world, so as to find the projection relationship between the two-dimensional world and the three-dimensional world, so as to realize the applications of pose estimation, camera correction and so on.

The aruco module in OpenCV includes the creation and detection of aruco tags, as well as the related API s for using aruco tags in applications such as pose estimation and camera correction. At the same time, it also provides tag boards and so on. In this note, we mainly organize the creation and detection of aruco tags.

First, when we create the aruco tag, we need to specify a dictionary first. This dictionary represents the size, coding and other contents of the created aruco tag. We use APIgetPredefinedDictionary () to declare the dictionary we use. In OpenCV, a variety of predefined dictionaries are provided. We can use PREDEFINED_DICTIONARY_NAME to see which predefined dictionaries are available. Moreover, the dictionary name indicates the number and size of aruco marks of the dictionary, such as DICT_7X7_50 means a dictionary containing 50 7x7 bit tags.

ArUco tag generator

Online aruco tag generator: http://aruco.dgut.top/

(standby): https://chev.me/arucogen/

Generate ArUco tags in OpenCV

Generate aruco tag with OpenCV Python

After determining the dictionary we need, we can draw the aruco mark through APIdrawMarker(), and its parameter meanings are as follows:

import cv2
import numpy as np
# Generate aruco tag
# Load predefined Dictionaries
dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)

# Generate tag
markerImage = np.zeros((200, 200), dtype=np.uint8)
markerImage = cv2.aruco.drawMarker(dictionary, 22, 200, markerImage, 1)
cv2.imwrite("marker22.png", markerImage)

The aruco module of opencv has 25 predefined tag dictionaries. All aruco tags in each dictionary contain the same number of blocks or bits (for example, 4) × 4,5 × 5,6 × 6 or 7 × 7) , and the number of aruco tags in each dictionary is fixed (e.g. 50, 100, 250 or 1000).

cv2. aruco. Dictionary_ The get () function loads CV2 aruco. DICT_ 6X6_ 250 dictionary containing 250 tags, where each tag is 6 × 6-bit binary mode

cv2. aruco. The second parameter 22 in drawmarker (Dictionary, 22, 200, markerImage, 1) is the mark id (0 ~ 249) of aruco, and the third parameter determines the size of the generated mark. In the above example, it will generate 200 × For 200 pixel image, the fourth parameter represents the object to store aruco tag (markerImage above). Finally, the fifth parameter is the boundary width parameter, which determines how many bits (blocks) should be added to the generated binary pattern as a boundary.

After execution, such tags will be generated: tag IDs are 22 respectively

Expand the supported tag dictionary

Expand the contents of the view;
Python: cv.aruco.DICT_4X4_50
Python: cv.aruco.DICT_4X4_100
Python: cv.aruco.DICT_4X4_250
Python: cv.aruco.DICT_4X4_1000
Python: cv.aruco.DICT_5X5_50
Python: cv.aruco.DICT_5X5_100
Python: cv.aruco.DICT_5X5_250
Python: cv.aruco.DICT_5X5_1000
Python: cv.aruco.DICT_6X6_50
Python: cv.aruco.DICT_6X6_100
Python: cv.aruco.DICT_6X6_250
Python: cv.aruco.DICT_6X6_1000
Python: cv.aruco.DICT_7X7_50
Python: cv.aruco.DICT_7X7_100
Python: cv.aruco.DICT_7X7_250
Python: cv.aruco.DICT_7X7_1000
Python: cv.aruco.DICT_ARUCO_ORIGINAL
Python: cv.aruco.DICT_APRILTAG_16h5
4x4 bits, minimum hamming distance between any two codes = 5, 30 codes

Batch generate aruco Tags

import cv2
import numpy as np
# Generate aruco tag
# Load predefined Dictionaries
dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)

# Generate tag
markerImage = np.zeros((200, 200), dtype=np.uint8)
for i in range(30):
    markerImage = cv2.aruco.drawMarker(dictionary, i, 200, markerImage, 1);

    cv2.imwrite(firename, markerImage);

A series of 6*6 aruco tags will be generated under the armark folder

Detection and location of Aruco markers

Static detection

Aruco markers are detected in the image in the environment, and there are 7 markers in the environment

import numpy as np
import time
import cv2
import cv2.aruco as aruco
#Read picture
#size pictures
#Grayscale words
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
#Set predefined dictionary
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
#Initialize detector parameters with default values
parameters =  aruco.DetectorParameters_create()
#Use aruco The detectmarkers() function can detect the marker and return the ID and the coordinates of the four corners of the sign board
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)
#Draw the position of the sign
aruco.drawDetectedMarkers(frame, corners,ids)


For each successful detection of the mark, the four corners of the mark will be detected in the order of top left, top right, bottom right and bottom left. In C + +, the four detected corners are stored as point vectors, and multiple marks in the image are stored together in the point vector container. In Python, they are stored as Numpy arrays.

The detectMarkers() function is used to detect and determine the position of the marked corner.

  • The first parameter image is the scene image with the tag.
  • The second parameter, dictionary, is the dictionary used to generate the tag. Successfully detected tags are stored in markerCorners and their ID s are stored in markerIds. The parameters passed as the parameters of the previous initialization object.
  • The third parameter: the object of DetectionParameters class, which includes all parameters that can be customized in the detection process;
  • Return parameter corners: the corner list of the detected aruco tag. For each tag, its four corners are returned in their original order (rotating clockwise from the upper right corner). The first corner is the upper right corner, followed by the lower right corner, lower left corner and upper left corner.
  • Return ids: the id of each tag detected. It should be noted that the third parameter and the fourth parameter have the same size;
  • Return parameter rejectedImgPoints: list of discarded candidate marks, that is, squares detected but not provided with valid codes. Each candidate mark is also defined by its four corners, and its format is the same as the third parameter, which can be omitted if there are no special requirements.
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)

After we detect the aruco tag, in order to facilitate observation, we need to carry out visual operation to mark the tag: use the API drawDetectedMarkers() to draw the detected aruco tag, and its parameter meanings are as follows:

  • Parameter image: is the input / output image of the marker to be drawn (usually the image with the marker detected)
  • Parameter corners: corner list of detected aruco markers
  • Parameter ids: each tag detected corresponds to the ID in its dictionary. Optional (if not provided) ID will not be drawn.
  • Parameter borderColor: draw the color of the outer frame of the mark, and the other colors (text color and first corner color) will be calculated based on this color to improve the visualization effect.
  • No return value
aruco.drawDetectedMarkers(image, corners,ids,borderColor)

Effect demonstration:

Dynamic detection

Use the camera to dynamically monitor the aruco mark in real time and estimate the posture. The internal parameters of the camera need to be calibrated in advance. Please see my other article on how to calibrate

import numpy as np
import time
import cv2
import cv2.aruco as aruco

# mtx = np.array([
#         [2946.48,       0, 1980.53],
#         [      0, 2945.41, 1129.25],
#         [      0,       0,       1],
#         ])
# #When my mobile phone takes a picture of the chessboard, the picture size is 4000 x 2250
# #The ip camera is set to 1920 x 1080 when shooting video, and the aspect ratio is the same,
# #Pay attention when setting the resolution of ip camera
# dist = np.array( [0.226317, -1.21478, 0.00170689, -0.000334551, 1.9892] )

#Camera correction parameters

# dist=np.array(([[-0.51328742,  0.33232725 , 0.01683581 ,-0.00078608, -0.1159959]]))
# mtx=np.array([[464.73554153, 0.00000000e+00 ,323.989155],
#  [  0.,         476.72971528 ,210.92028],
#  [  0.,           0.,           1.        ]])
dist=np.array(([[-0.58650416 , 0.59103816, -0.00443272 , 0.00357844 ,-0.27203275]]))
newcameramtx=np.array([[189.076828   ,  0.    ,     361.20126638]
 ,[  0 ,2.01627296e+04 ,4.52759577e+02]
 ,[0, 0, 1]])
mtx=np.array([[398.12724231  , 0.      ,   304.35638757],
 [  0.       ,  345.38259888, 282.49861858],
 [  0.,           0.,           1.        ]])

cap = cv2.VideoCapture(0)

font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below)

#num = 0
while True:
    ret, frame = cap.read()
    h1, w1 = frame.shape[:2]
    # Read the camera picture
    # Correct distortion
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (h1, w1), 0, (h1, w1))
    dst1 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
    x, y, w1, h1 = roi
    dst1 = dst1[y:y + h1, x:x + w1]

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
    parameters =  aruco.DetectorParameters_create()
    dst1 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
        detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI
        mgPoints]]]]) -> corners, ids, rejectedImgPoints

    #Use aruco The detectmarkers() function can detect the marker and return the ID and the coordinates of the four corners of the sign board
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)

#    If you can't find it, type id
    if ids is not None:

        rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, 0.05, mtx, dist)
        # Estimate the attitude of each marker and return the values rvet and tvec --- different
        # from camera coeficcients
        (rvec-tvec).any() # get rid of that nasty numpy value array error

#        aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.1) #Draw axis
#        aruco.drawDetectedMarkers(frame, corners) #Draw a square around the mark

        for i in range(rvec.shape[0]):
            aruco.drawAxis(frame, mtx, dist, rvec[i, :, :], tvec[i, :, :], 0.03)
            aruco.drawDetectedMarkers(frame, corners)
        ###### DRAW ID #####
        cv2.putText(frame, "Id: " + str(ids), (0,64), font, 1, (0,255,0),2,cv2.LINE_AA)

        ##### DRAW "NO IDS" #####
        cv2.putText(frame, "No Ids", (0,64), font, 1, (0,255,0),2,cv2.LINE_AA)

    # Display result frame

    key = cv2.waitKey(1)

    if key == 27:         # Press esc to exit
        print('esc break...')

    if key == ord(' '):   # Press the spacebar to save
#        num = num + 1
#        filename = "frames_%s.jpg" % num  # Save an image
        filename = str(time.time())[:10] + ".jpg"
        cv2.imwrite(filename, frame)


Blog address: https://blog.dgut.top/2020/07/15/python-aruco/




Added by xcmir on Fri, 04 Mar 2022 04:20:01 +0200