python写一个程序,把在左相机下的得到的16个世界坐标列表根据左右相机双目标定得到的外参转到右相机下的世界坐标
时间: 2023-06-17 21:07:01 浏览: 81
python写的坐标转换程序
假设你已经得到了左右相机的内参和外参,并且已经得到了左相机下的16个世界坐标列表,那么可以按照以下步骤将它们转换到右相机下的世界坐标:
1. 将左相机下的坐标系转换到右相机下的坐标系。
这里需要用到左右相机的外参,可以通过以下代码实现:
```python
import numpy as np
# 左右相机的外参
R = np.array([[0.9996, 0.025, -0.013],
[-0.025, 0.9996, -0.002],
[0.013, 0.002, 1]])
T = np.array([[-100],
[0],
[0]])
# 左相机到右相机的旋转和平移
R_lr = np.linalg.inv(R)
T_lr = -R_lr.dot(T)
```
2. 将左相机下的世界坐标转换为左相机下的相机坐标。
这里需要用到左相机的内参,可以通过以下代码实现:
```python
# 左相机的内参
K = np.array([[534.89, 0, 318.97],
[0, 534.89, 239.91],
[0, 0, 1]])
# 左相机下的世界坐标
world_points = np.array([[0, 0, 0],
[0.05, 0, 0],
[0, 0.05, 0],
[0, 0, 0.05],
[0.05, 0.05, 0],
[0.05, 0, 0.05],
[0, 0.05, 0.05],
[0.05, 0.05, 0.05],
[0.1, 0, 0],
[0, 0.1, 0],
[0, 0, 0.1],
[0.1, 0.1, 0],
[0.1, 0, 0.1],
[0, 0.1, 0.1],
[0.1, 0.1, 0.1],
[0.05, 0.05, 0.1]])
# 将世界坐标转换为相机坐标
camera_points = np.dot(R, world_points.T) + T
```
3. 将左相机下的相机坐标转换为右相机下的相机坐标。
这里需要用到左右相机的相对位置关系,即左相机到右相机的旋转和平移,可以通过第1步中得到的旋转和平移计算得到:
```python
# 将左相机下的相机坐标转换为右相机下的相机坐标
camera_points_lr = np.dot(R_lr, camera_points) + T_lr
```
4. 将右相机下的相机坐标转换为右相机下的世界坐标。
这里需要用到右相机的内参,可以通过以下代码实现:
```python
# 右相机的内参
K_lr = np.array([[534.89, 0, 318.97],
[0, 534.89, 239.91],
[0, 0, 1]])
# 将右相机下的相机坐标转换为右相机下的世界坐标
world_points_lr = np.dot(np.linalg.inv(K_lr), camera_points_lr)
```
最终得到的`world_points_lr`就是左相机下的16个世界坐标列表根据左右相机双目标定得到的外参转到右相机下的世界坐标。
阅读全文