Autonomous vehicle 杂谈_01

一. 坐标转换的过程

1. 总过程：
   
2. 分步实现
   世界坐标转换为相机坐标
   
   相机坐标转换为图像坐标
   
   图像坐标转换为像素坐标
   

PS： 世界坐标： (Xw, Yw, Zw)、 相机坐标：(Xc, Yc, Zc)、 图像坐标：(x, y)、 像素坐标： (u, v)、 相机焦距： f、 旋转矩阵：R、 平移矩阵： T

PS: 本文代码不够准确，请移步至博主更新版博文：Python实现激光雷达点云的向图像的重投影（世界坐标转换为像素坐标 + Autoware标定结果使用方法）- - 附源码 * 更新版 - -

二. 需要做的前期准备

1. 通过 calibration 得到的相机内外参数
2. set up 一个用于存放py源码(.py)、待转换三维坐标(.txt)__ps： 格式为（Xw，Yw，Zw）、转换二维坐标结果(.txt)的文件夹，大概就是像下边图片中的一样
   
3. 随便一个可以run python文件的IDE

三. 源码如下（需要更改自己的相机参数）

# 实现三维坐标向二维坐标的转换 

import numpy as np

"""相机内、外参矩阵"""

# 外参矩阵 (需要改)
Out = np.mat([
    [-0.117, -0.992, 0.028, -0.125],
    [-0.0033, -0.0278, -0.9996, 0.2525],
    [0.993, -0.1174, 0.00000315, 0.0716],
    [0, 0, 0, 1]
])

# 内参矩阵 (需要改)
K = np.mat([
    [610.53, 0, 368.114],
    [0, 605.93, 223.969],
    [0, 0, 1]
])

"""坐标转换"""
# 打开用于存放世界坐标的txt文件，将其中的以字符串格式保存的世界坐标转换成（Xw， Yw， Zw， 1）的元组格式
f = open('database', 'r')
database = []
for line in f.readlines():
    coordinate = line.strip()  # 去掉左右的空格符
    coordinate = eval(coordinate)  # 将字符串格式的坐标转换为元组格式
    database.append(coordinate)
# print(database)

world_coordinate_list = []
for item in database:
    world_coordinate_part = (item[0], item[1], item[2], 1)
    world_coordinate_list.append(world_coordinate_part)
# print(world_coordinate_list)


pixel_coordinate_list = []

for item in world_coordinate_list:
    world_coordinate = np.mat([
        [item[0]],
        [item[1]],
        [item[2]],
        [item[3]]
    ])
    print(f'世界坐标为：\n{world_coordinate}')
    # print(type(world_coordinate))
    
    # 世界坐标系转换为相加坐标系 （Xw,Yw,Zw）--> (Xc,Yc,Zc)
    camera_coordinate = Out * world_coordinate
    print(f'相机坐标为：\n{camera_coordinate}')
    Zc = float(camera_coordinate[2])
    print(f'Zc={Zc}')

    # 相机坐标系转图像坐标系 (Xc,Yc,Zc) --> (x, y)  下边的f改为焦距
    focal_length = np.mat([
        [f, 0, 0, 0],
        [0, f, 0, 0],
        [0, 0, 1, 0]
    ])
    image_coordinate = (focal_length * camera_coordinate) / Zc
    print(f'图像坐标为：\n{image_coordinate}')

    # 图像坐标系转换为像素坐标系
    pixel_coordinate = K * image_coordinate
    print(f'像素坐标为：\n{pixel_coordinate}')
    pixel_coordinate_list.append(pixel_coordinate)
    print('---------------------分割线--------------------------------')

print(pixel_coordinate_list)
f = open("result.txt", "w", encoding="utf-8")
for item in pixel_coordinate_list:
    f.write(str(item)+'\n')
    f.write('------------分割线-----------------'+'\n')
f.close()

坐标转换方法参考：https://blog.csdn.net/guyuealian/article/details/104184551

如有问题，敬请指正。欢迎转载，但请注明出处。