openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加四元数和欧拉角解算

Browse Source
This commit is contained in:
shenchenyang 2026-02-05 19:27:23 +08:00
parent bb7115b5a5
commit a2fc8c1faf
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
View File

@ -9,6 +9,8 @@ from geometry_msgs.msg import PointStamped
from cv_bridge import CvBridge from cv_bridge import CvBridge
from sensor_msgs.msg import CameraInfo from sensor_msgs.msg import CameraInfo
import mediapipe as mp import mediapipe as mp
from scipy.spatial.transform import Rotation as R
# from tf_transformations import quaternion_from_euler, euler_from_quaternion
class StereoHandDepthNode(Node): class StereoHandDepthNode(Node):
@ -181,8 +183,14 @@ class StereoHandDepthNode(Node):
cameraMatrix=self.K1, cameraMatrix=self.K1,
distCoeffs=self.D1 distCoeffs=self.D1
) )
if(ret == True):
cv2.drawFrameAxes(left_gray, self.K1, self.D1, rvec, tvec, 0.1, 2) cv2.drawFrameAxes(left_gray, self.K1, self.D1, rvec, tvec, 0.1, 2)
# self.get_logger().info(f"rvec: {rvec}") R_ca, _ = cv2.Rodrigues(rvec)
rot = R.from_matrix(R_ca)
qx, qy, qz, qw = rot.as_quat()
roll, pitch, yaw = rot.as_euler('xyz')
self.get_logger().info(f"quat: ({qx}, {qy}, {qz}, {qw})")
self.get_logger().info(f"euler: ({roll*180/3.14:.1f}, {pitch*180/3.14:.1f}, {yaw*180/3.14:.1f})")
# u = int(u * (w / 2)) # 注意这里是左图宽度 # u = int(u * (w / 2)) # 注意这里是左图宽度
# v = int(v * h) # v = int(v * h)
p.header = msg.header p.header = msg.header