任意两不同坐标系的相互转换

本文详细介绍了施工坐标系向测量坐标系转换的方法,并特别强调了旋转角度a的重要性。该文主要聚焦于机器人及其传感器坐标系之间的转换,特别是激光雷达传感器与机器人坐标系之间的相互转换。

 注意: 应该用施工坐标系向测量坐标系旋转的夹角为a。这个公式的用在机器人的本身坐标系同机器人传感器坐标系之间的互相转换。尤其是激光雷达传感器同机器人之间相互转化。

### 不同世界坐标系之间的转换 在多传感器融合或多视角几何的应用中,经常遇到需要将在某个世界坐标系下的点转换到另一个世界坐标系下。这种转换通常涉及到两个主要操作:平移和旋转。 假设存在两个世界坐标系 \( W_{A} \) 和 \( W_{B} \),其中 \( W_{A} \) 中的一个点表示为向量 \( P_A=(X_A,Y_A,Z_A)^{T} \),而该点在 \( W_{B} \) 下的位置则由 \( P_B=(X_B,Y_B,Z_B)^{T} \) 表示。为了实现从 \( W_{A} \) 到 \( W_{B} \) 的转换,可以通过定义一个齐次变换矩阵来完成此过程[^4]: \[ T_{AB}= \begin{bmatrix} R & t\\ 0^{T}& 1 \end{bmatrix}\] 这里, - \( R \) 是一个 \( 3\times3 \) 的正交矩阵,代表了坐标系间的相对旋转; - \( t=[t_x,t_y,t_z]^T \) 是一个三维列向量,描述的是原点位移; 对于任意给定点 \( P_A \),其对应的 \( P_B \) 可通过如下方式计算得到: \[ P_B=T_{AB}*P_A=\left( \begin{array}{c|c} R&t \\ \hline 0&1 \end{array} \right)\times\left(\begin{array}{c} P_a \\ 1 \end{array}\right)=RP_a+t \] 如果要反方向地把 \( W_{B} \) 转换成 \( W_{A} \),那么就需要求解上述变换矩阵的逆矩阵 \( T^{-1}_{BA} \): \[ T^{-1}_{BA} = (T_{AB})^{-1} = \begin{pmatrix} R^T & -R^T*t\\ 0^T & 1 \end{pmatrix}\] 这表明,在已知 \( W_{A} \) 对应于 \( W_{B} \) 的姿态参数的情况下,可以直接利用这些信息来进行相互间坐标的快速切换。 ```python import numpy as np def transform_point(T, point): """ Transforms a single homogeneous coordinate from one world frame to another. Parameters: T : ndarray of shape (4, 4), transformation matrix between two frames. point : array_like with length 3 or 4, the coordinates of the point in source frame. Returns: transformed_point : ndarray of shape (3,), the new position after applying the transformation. """ if len(point) == 3: homog_point = np.append(point, 1) elif len(point) == 4 and point[-1]==1: homog_point = point else: raise ValueError('Invalid input format') result_homogeneous = T @ homog_point return result_homogeneous[:3]/result_homogeneous[3] # Example usage rotation_matrix_example = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] translation_vector_example = [-1., 2., .5] transformation_matrix_AB = np.block([ [np.array(rotation_matrix_example), translation_vector_example], [0, 0, 0, 1]]) point_in_world_A = [1, 2, 3] transformed_point_to_world_B = transform_point(transformation_matrix_AB, point_in_world_A) print(f"The point {point_in_world_A} transforms into {list(transformed_point_to_world_B)} under given rotation and translation.") ```
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值