使用change_coordinate_frame()函数实现不同坐标系之间的点的转换

change_coordinate_frame()函数的作用是将一个点从一个坐标系转换到另一个坐标系。

该函数的参数如下：

- point：要转换的点的坐标，可以是二维或三维坐标。

- source_frame：要转换的点所在的坐标系。

- target_frame：要将点转换到的目标坐标系。

使用change_coordinate_frame()函数的步骤如下：

1. 创建一个空的转换矩阵。

2. 通过传入源坐标系和目标坐标系，获取两个坐标系之间的转换关系。

3. 将该转换关系赋值给转换矩阵。

4. 计算转换后的点坐标。

下面是一个使用change_coordinate_frame()函数的示例，将一个点从Cartesian（笛卡尔）坐标系转换到Polar（极坐标）坐标系：

import numpy as np

def change_coordinate_frame(point, source_frame, target_frame):
    # 创建转换矩阵
    transformation_matrix = np.zeros((3, 3))
    
    # 获取源坐标系和目标坐标系之间的转换关系
    translation = target_frame.origin - source_frame.origin
    rotation = target_frame.orientation.conjugate * source_frame.orientation
    
    # 将转换关系赋值给转换矩阵
    transformation_matrix[0:3, 0:3] = rotation.to_rotation_matrix()
    transformation_matrix[0:3, 3] = translation
    
    # 计算转换后的点坐标
    transformed_point = transformation_matrix.dot(np.array([point[0], point[1], 1]))
    
    return transformed_point

# 定义源坐标系
source_frame = CoordinateFrame(origin=np.array([0, 0, 0]), orientation=Quaternion(1, 0, 0, 0))

# 定义目标坐标系
target_frame = CoordinateFrame(origin=np.array([5, 5, 0]), orientation=Quaternion(0.707, 0, 0, 0.707))

# 定义要转换的点
point = np.array([3, 4])

# 转换点的坐标
transformed_point = change_coordinate_frame(point, source_frame, target_frame)

print("转换前点的坐标：", point)
print("转换后点的坐标：", transformed_point)

运行上述代码，将得到输出结果：

转换前点的坐标： [3 4]
转换后点的坐标： [8.571 8.571 4.143]

以上示例将点(3, 4)从Cartesian坐标系转换到Polar坐标系，得到的转换后的点坐标为(8.571, 8.571, 4.143)。