opencv ArUco标记,姿势估计-准确地为哪个点提供平移和旋转?

pwuypxnk  于 2023-01-17  发布在  其他
关注(0)|答案(1)|浏览(162)

我检测了ArUco标记并估计了姿势。见下图。但是,我得到的Xt(X平移)是正值。根据drawAxis函数,正方向远离图像中心。所以我认为它应该是负值。为什么我得到的是正值。
我的相机距离成像表面约120 mm。但我得到的Zt(Z平移)在650 mm范围内。位姿估计是否给出了标记相对于物理相机或图像平面中心的位姿?我不明白Zt为什么这么高。
我在改变Z的同时不断测量姿势,并获得了滚转、俯仰、偏航。我注意到滚转(旋转w.r.t.凸轮X轴)的符号在幅度变化166-178之间来回变化,但Xt的符号并没有随着滚转的符号变化而变化。有什么想法吗?有什么建议可以获得更一致的数据吗?

image=cv.imread(fname)
arucoDict = cv.aruco.Dictionary_get(cv.aruco.DICT_4X4_1000)
arucoParams = cv.aruco.DetectorParameters_create()
(corners, ids, rejected) = cv.aruco.detectMarkers(image, arucoDict,
    parameters=arucoParams)

    print(corners, ids, rejected)
    
    if len(corners) > 0:
        # flatten the ArUco IDs list
        ids = ids.flatten()
        # loop over the detected ArUCo corners
        #for (markerCorner, markerID) in zip(corners, ids):
        #(markerCorner, markerID)=(corners, ids)
            # extract the marker corners (which are always returned in
            # top-left, top-right, bottom-right, and bottom-left order)
        #corners = corners.reshape((4, 2))
        (topLeft, topRight, bottomRight, bottomLeft) = corners[0][0][0],corners[0][0][1],corners[0][0][2],corners[0][0][3]
            # 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
        cv.line(image, topLeft, topRight, (0, 255, 0), 2)
        cv.line(image, topRight, bottomRight, (0, 255, 0), 2)
        cv.line(image, bottomRight, bottomLeft, (0, 255, 0), 2)
        cv.line(image, bottomLeft, topLeft, (0, 255, 0), 2)
            # compute and draw the center (x, y)-coordinates of the ArUco
            # marker
        cX = int((topLeft[0] + bottomRight[0]) / 2.0)
        cY = int((topLeft[1] + bottomRight[1]) / 2.0)
        cv.circle(image, (cX, cY), 4, (0, 0, 255), -1)
        
        if topLeft[1]!=topRight[1] or topLeft[0]!=bottomLeft[0]:
            rot1=np.degrees(np.arctan((topLeft[0]-bottomLeft[0])/(bottomLeft[1]-topLeft[1])))
            rot2=np.degrees(np.arctan((topRight[1]-topLeft[1])/(topRight[0]-topLeft[0])))
            rot=(np.round(rot1,3)+np.round(rot2,3))/2
            print(rot1,rot2,rot)
        else:
            rot=0
    
        # draw the ArUco marker ID on the image
        rotS=",rotation:"+str(np.round(rot,3))
        cv.putText(image, ("position: "+str(cX) +","+str(cY)),
        (100, topLeft[1] - 15), cv.FONT_HERSHEY_SIMPLEX,0.5, (255, 0, 80), 2)
        cv.putText(image, rotS,
        (400, topLeft[1] -15), cv.FONT_HERSHEY_SIMPLEX,0.5, (255, 0, 80), 2)
        print("[INFO] ArUco marker ID: {}".format(ids))
        
        
        d=np.round((math.dist(topLeft,bottomRight)+math.dist(topRight,bottomLeft))/2,3)
        # Get the rotation and translation vectors
        rvecs, tvecs, obj_points = cv.aruco.estimatePoseSingleMarkers(corners,aruco_marker_side_length,mtx,dst)
        
         
        # Print the pose for the ArUco marker
        # The pose of the marker is with respect to the camera lens frame.
        # Imagine you are looking through the camera viewfinder, 
        # the camera lens frame's:
        # x-axis points to the right
        # y-axis points straight down towards your toes
        # z-axis points straight ahead away from your eye, out of the camera
        #for i, marker_id in enumerate(marker_ids):
         
            # Store the translation (i.e. position) information
        transform_translation_x = tvecs[0][0][0]
        transform_translation_y = tvecs[0][0][1]
        transform_translation_z = tvecs[0][0][2]
               
            # Store the rotation information
        rotation_matrix = np.eye(4)
        rotation_matrix[0:3, 0:3] = cv.Rodrigues(np.array(rvecs[0]))[0]
        r = R.from_matrix(rotation_matrix[0:3, 0:3])
        quat = r.as_quat()   
             
            # Quaternion format     
        transform_rotation_x = quat[0] 
        transform_rotation_y = quat[1] 
        transform_rotation_z = quat[2] 
        transform_rotation_w = quat[3] 
             
            # Euler angle format in radians
        roll_x, pitch_y, yaw_z = euler_from_quaternion(transform_rotation_x,transform_rotation_y,transform_rotation_z,transform_rotation_w)
             
        roll_x = math.degrees(roll_x)
        pitch_y = math.degrees(pitch_y)
        yaw_z = math.degrees(yaw_z)

j9per5c4

j9per5c41#

  • 免责声明:这适用于OpenCV v4.5.5和相应的aruco模块(contrib repo)。他们为v4.6.0和v4.7.0重做了很多aruco的东西,所以最好检查我在这里说的一切。*

无需检查所有代码(看起来大致可以),下面介绍一些关于OpenCV和aruco的基础知识:
两者都使用右手坐标系:拇指X,食指Y,中间Z。
OpenCV使用X右,Y下,Z远来表示屏幕/相机帧。屏幕和图片的原点是左上角。对于相机,原点是针孔模型的中心,也就是光圈的中心。我不能对镜头或镜头系统进行评论。假设镜头中心是原点。这可能已经足够接近了。
如果标记平放在桌面上,Aruco使用X右、Y远、Z上。原点位于标记的中心。标记的左上角被视为“第一”角。
可以认为标记具有其自己的坐标系/框架。
rvec和tvec给出的姿态是相机帧 * 中标记 * 的姿态。这意味着np.linalg.norm(tvec)给出了从相机到标记中心的直接距离。tvec的Z只是平行于光轴的分量。
如果标记位于图片的右半部分(“一半”由摄像机矩阵的cx、cy定义),你会期望tvec的X增长,下半部分,Y为正/增长。
相反,这种转换会将标记局部坐标转换为相机局部坐标。尝试转换一些标记局部点,如原点或轴上的点。我相信cv::transform可以帮助实现这一点。使用OpenCV的projectPoints将3D空间点Map为2D图像点,然后可以绘制标记的轴,或在其上绘制立方体,或任何您喜欢的东西。
假设标记直立并正对着摄影机。当您考虑空间(“世界”空间)中标记和摄影机的帧三元组时,两者都是X“右”,但其中一个的Y和Z与另一个的Y和Z相反,因此您可能会看到围绕X轴旋转半圈(旋转Z和Y)。
你可以想象这样的转变:

  • 最初,照相机透过标记物从标记物的背面向外看世界。照相机将是“颠倒的”。照相机看到标记物空间。
  • 姿势的旋转组件围绕摄像机原点旋转整个标记局部世界。从世界坐标系(参考点)看,* 摄像机 * 旋转到你觉得自然的姿态。
  • 姿势的平移将标记的世界移出摄影机前面(Z为正),或者等效地,摄影机从标记后退。

如果你得到的值不可信,检查aruco_marker_side_length和相机矩阵。对于典型的分辨率(VGA-4k)和视野(60-80度),f大约是500-3000。

相关问题