我检测了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)
1条答案
按热度按时间j9per5c41#
无需检查所有代码(看起来大致可以),下面介绍一些关于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)。
你可以想象这样的转变:
如果你得到的值不可信,检查
aruco_marker_side_length
和相机矩阵。对于典型的分辨率(VGA-4k)和视野(60-80度),f大约是500-3000。