已知欧氏距离与四元数旋转，求解3D点坐标及Python实现方法

已知：

• Point 1在原点坐标系下的坐标为 (1, 2, 3)，其局部坐标系相对于原点的旋转四元数为 w,x,y,z=(0.8, 0.1, 0.1, 0.1)
• Point 2与Point 1的欧氏距离为2，且Point 2的局部坐标系相对于Point 1的旋转四元数为 w,x,y,z=(0.5, 0.5, 0.5, 0.5)

需计算Point 2在原点坐标系下的(x,y,z)坐标，并提供Python实现方案。

要得到Point 2的全局坐标，需完成以下步骤：

1. 确保输入四元数为单位四元数（旋转四元数必须归一化，否则会引入缩放误差）
2. 将Point 1的旋转四元数转换为旋转矩阵，用于将Point 1局部坐标系下的向量转换到原点坐标系
3. 定义Point 2在Point 1局部坐标系中的偏移向量（由于距离为2，可选择沿局部y轴的(0,2,0)，与RVIZ可视化设置一致）
4. 通过旋转矩阵将局部偏移向量转换到原点坐标系
5. 将转换后的偏移向量与Point 1的全局坐标相加，得到Point 2的全局坐标

首先安装依赖库：

pip install numpy quaternion

实现代码：

import numpy as np
import quaternion

# 已知参数
p1_global_coords = np.array([1, 2, 3])
# Point1局部坐标系相对于原点的旋转四元数（quaternion库构造顺序：w, x, y, z）
p1_rot_quat = np.quaternion(0.8, 0.1, 0.1, 0.1)
# Point2局部坐标系相对于Point1的旋转四元数
p2_rot_quat = np.quaternion(0.5, 0.5, 0.5, 0.5)
# 两点间欧氏距离
distance = 2

# 归一化四元数，确保为单位四元数
p1_rot_quat = p1_rot_quat.normalized()
p2_rot_quat = p2_rot_quat.normalized()

# 将Point1的旋转四元数转换为旋转矩阵（局部→全局）
p1_rot_matrix = quaternion.as_rotation_matrix(p1_rot_quat)

# 定义Point2在Point1局部坐标系中的偏移向量（沿局部y轴，长度为distance）
p2_local_offset = np.array([0, distance, 0])

# 将局部偏移向量转换到原点坐标系
p2_global_offset = p1_rot_matrix @ p2_local_offset

# 计算Point2的全局坐标
p2_global_coords = p1_global_coords + p2_global_offset

print(f"Point2全局坐标：x={p2_global_coords[0]:.4f}, y={p2_global_coords[1]:.4f}, z={p2_global_coords[2]:.4f}")

已实现基于ROS 2的TF广播代码，可直观验证计算结果：

import time
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped, Vector3, Quaternion

class TestAnimator(Node):
    def __init__(self):
        super().__init__('TestAnimator')
        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)   
        self.timer = self.create_timer(1, self.timer_callback)
                    
    def timer_callback(self):      
        # 发布Point1相对于原点的TF
        p1_tf = TransformStamped()
        p1_tf.header.frame_id = 'origin'
        p1_tf.child_frame_id = 'Point_1'
        p1_tf.transform.translation = Vector3(x=1., y=2., z=3.)
        p1_tf.transform.rotation = Quaternion(w=0.8, x=0.1, y=0.1, z=0.1)
        self.tf_broadcaster.sendTransform(p1_tf)
        
        # 发布Point2相对于Point1的TF
        p2_tf = TransformStamped()
        p2_tf.header.frame_id = 'Point_1'
        p2_tf.child_frame_id = 'Point_2'
        p2_tf.transform.translation = Vector3(x=0., y=2., z=0.)
        p2_tf.transform.rotation = Quaternion(w=0.5, x=0.5, y=0.5, z=0.5)
        self.tf_broadcaster.sendTransform(p2_tf)
        
        time.sleep(0.05)     

if __name__ == '__main__':
    rclpy.init(args=None)
    test_animator = TestAnimator()
    rclpy.spin(test_animator)

可视化说明：

• 右下角坐标系为原点(0,0,0)
• Point1的坐标系相对于原点倾斜，对应给定的旋转四元数
• Point2在Point1局部坐标系中沿y轴偏移2个单位，其在RVIZ中的全局位置与代码计算结果一致

内容的提问来源于stack exchange，提问作者HaakonFlaar