关于变换矩阵的计算

一、有中间商的

根据提供的信息，我们需要计算从wamv/lidar_wamv_link到wamv/middle_right_camera_link_optical的变换矩阵。已知wamv/lidar_wamv_link和wamv/middle_right_camera_link_optical相对于wamv/base_link的平移和旋转信息，我们将分别使用平移向量和RPY角度来计算这个变换矩阵。

1. 相对于base_link的平移向量

对于wamv/lidar_wamv_link：

tlidar​=[0.700,0.000,1.800]

对于wamv/middle_right_camera_link_optical：

tcamera​=[0.500,−0.450,1.500]

从lidar到camera的平移向量：

tlidar_to_camera​=tcamera​−tlidar​=[0.500−0.700,−0.450−0.000,1.500−1.800]=[−0.200,−0.450,−0.300]

2. 相对于base_link的旋转信息（RPY角度）

对于wamv/lidar_wamv_link，其RPY角度为：

RPYlidar​=[0.000,0.000,0.000](度数)

对于wamv/middle_right_camera_link_optical，其RPY角度为：

RPYcamera​=[−105.000,0.000,−180.000](度数)

从lidar到camera的旋转角度：

RPYlidar_to_camera​=RPYcamera​−RPYlidar​=[−105.000−0.000,0.000−0.000,−180.000−0.000]=[−105.000,0.000,−180.000]

3. 计算旋转矩阵

使用RPY角度计算旋转矩阵。旋转矩阵由三个基本旋转矩阵（绕x轴、y轴和z轴的旋转）相乘得到：

R=Rz​(Yaw)⋅Ry​(Pitch)⋅Rx​(Roll)

3.1 绕x轴旋转（Roll）

Rx​(−105∘)=​100​0cos(−105∘)sin(−105∘)​0−sin(−105∘)cos(−105∘)​​

计算值：

cos(−105∘)=cos(105∘)=−0.2588sin(−105∘)=−sin(105∘)=−0.9659

所以：

Rx​(−105∘)=​100​0−0.2588−0.9659​00.9659−0.2588​​

3.2 绕y轴旋转（Pitch）

Ry​(0∘)=​100​010​001​​

3.3 绕z轴旋转（Yaw）

Rz​(−180∘)=​cos(−180∘)sin(−180∘)0​−sin(−180∘)cos(−180∘)0​001​​

计算值：

cos(−180∘)=−1sin(−180∘)=0

所以：

Rz​(−180∘)=​−100​0−10​001​​

3.4 组合旋转矩阵

将三个旋转矩阵相乘：

R=Rz​(−180∘)⋅Ry​(0∘)⋅Rx​(−105∘)

计算步骤：

1. 先计算 Ry​(0∘)⋅Rx​(−105∘)：

Ry​(0∘)⋅Rx​(−105∘)=​100​010​001​​⋅​100​0−0.2588−0.9659​00.9659−0.2588​​=​100​0−0.2588−0.9659​00.9659−0.2588​​

1. 再计算 Rz​(−180∘)⋅(Ry​(0∘)⋅Rx​(−105∘))：

Rz​(−180∘)⋅​100​0−0.2588−0.9659​00.9659−0.2588​​=​−100​0−10​001​​⋅​100​0−0.2588−0.9659​00.9659−0.2588​​

计算结果：

R=​−100​00.25880.9659​0−0.96590.2588​​

4. 变换矩阵

将平移向量和旋转矩阵组合成齐次变换矩阵：

T=[R0​tlidar_to_camera​1​]=​−1 0 0 0​ 0 0.2588 0.9659 0​ 0 −0.9659 0.2588 0​ −0.200 −0.450 −0.300 1​​(四个四个是一列这样读)

这个矩阵描述了从wamv/lidar_wamv_link到wamv/middle_right_camera_link_optical的变换关系。

二、没有中间商的

1. 平移向量

平移向量为：

t=[−0.200,−0.450,−0.300]

2. RPY 角度

RPY 角度（弧度）为：

Roll=−1.833弧度Pitch=0.000弧度Yaw=−3.142弧度

转换为度数：

Roll=−105∘Pitch=0∘Yaw=−180∘

3. 旋转矩阵

根据 RPY 角度计算旋转矩阵。旋转矩阵由三个基本旋转矩阵（绕 x 轴、y 轴和 z 轴的旋转）相乘得到：

R=Rz​(Yaw)⋅Ry​(Pitch)⋅Rx​(Roll)

3.1 绕 x 轴旋转（Roll）

Rx​(Roll)=​100​0cos(Roll)sin(Roll)​0−sin(Roll)cos(Roll)​​

代入 Roll = -105°：

Rx​(−105∘)=​100​0cos(−105∘)sin(−105∘)​0−sin(−105∘)cos(−105∘)​​

计算值：

cos(−105∘)=cos(105∘)=−0.2588sin(−105∘)=−sin(105∘)=−0.9659

所以：

Rx​(−105∘)=​100​0−0.2588−0.9659​00.9659−0.2588​​

3.2 绕 y 轴旋转（Pitch）

Ry​(Pitch)=​cos(Pitch)0−sin(Pitch)​010​sin(Pitch)0cos(Pitch)​​

代入 Pitch = 0°：

Ry​(0∘)=​100​010​001​​

3.3 绕 z 轴旋转（Yaw）

Rz​(Yaw)=​cos(Yaw)sin(Yaw)0​−sin(Yaw)cos(Yaw)0​001​​

代入 Yaw = -180°：

Rz​(−180∘)=​cos(−180∘)sin(−180∘)0​−sin(−180∘)cos(−180∘)0​001​​

计算值：

cos(−180∘)=−1sin(−180∘)=0

所以：

Rz​(−180∘)=​−100​0−10​001​​

3.4 组合旋转矩阵

将三个旋转矩阵相乘：

R=Rz​(−180∘)⋅Ry​(0∘)⋅Rx​(−105∘)

计算步骤：

1. 先计算 Ry​(0∘)⋅Rx​(−105∘)：

Ry​(0∘)⋅Rx​(−105∘)=​100​010​001​​⋅​100​0−0.2588−0.9659​00.9659−0.2588​​=​100​0−0.2588−0.9659​00.9659−0.2588​​

1. 再计算 Rz​(−180∘)⋅(Ry​(0∘)⋅Rx​(−105∘))：

Rz​(−180∘)⋅​100​0−0.2588−0.9659​00.9659−0.2588​​=​−100​0−10​001​​⋅​100​0−0.2588−0.9659​00.9659−0.2588​​

计算结果：

R=​−100​00.25880.9659​0−0.96590.2588​​

4. 变换矩阵

将平移向量和旋转矩阵组合成齐次变换矩阵：

T=[R0​t1​]=​−1000​00.25880.96590​0−0.96590.25880​−0.200−0.450−0.3001​​

这个矩阵描述了从/wamv/lidar_wamv_link到/wamv/middle_right_camera_link_optical的变换关系。