一起做双目测距-USB_CAMERA检测人脸距离系列(3)-三角化(三角测距)的python实现(SLAM十四讲）_slam14讲python代码

三角化函数

阅读SLAM 十四讲，C++ 版本的三角化代码高翔博士已经实现了一遍，有时候因为代码需要，我们把其迁移到python上，看看python上的三角化

由于Python里面numpy用的比较多，没有C++ 的std::vector,所以用numpy数组替代vector存储二维像素点(u,v)即可：
函数如下：

# Triangulation.py
import cv2
import numpy as np
from Calib import USB_Camera

def triangulation(pointl_vec,pointr_vec,R,t,cam_matrix_left,cam_matrix_right,n):
    pointl_cam_vec = []
    pointr_cam_vec = []
    for pointl in pointl_vec:
        pointl_cam_vec.append([(pointl[0] - cam_matrix_left[0, 2]) / cam_matrix_left[0, 0],(pointl[1] - cam_matrix_left[1, 2]) / cam_matrix_left[1, 1]])
    for pointr in pointr_vec:
        pointr_cam_vec.append([(pointr[0] - cam_matrix_right[0, 2]) / cam_matrix_right[0, 0],(pointr[1] - cam_matrix_right[1, 2]) / cam_matrix_right[1, 1]])
    T1 = np.array([[1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.]])
    T2 = np.concatenate((R,t),axis=1)
    # print(T1,T2)
    pointl_cam_vec = np.array(pointl_cam_vec).transpose()
    pointr_cam_vec = np.array(pointr_cam_vec).transpose()
    # print(pointl_cam_vec)
    # print(pointr_cam_vec)

    pts_4d = np.zeros((4,n))
    cv2.triangulatePoints(T1,T2,pointl_cam_vec,pointr_cam_vec,pts_4d)
    pts_3d = []
    for i in range(n):
        x = pts_4d[0,i]/pts_4d[3,i]
        y = pts_4d[1,i]/pts_4d[3,i]
        z = pts_4d[2,i]/pts_4d[3,i]
        pts_3d.append([x,y,z])
    pts_3d = np.array(pts_3d)

    return pts_3d
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

相机参数类

1、首先要用这个函数，要知道传入参数的类型，这里给出一个测试实例
2、其次USB_Camera 这个类也提供如下，方便导入

# Calib.py
import numpy as np

class USB_Camera(object): 
    def __init__(self):
        # 左相机内参 及 畸变系数
        self.cam_matrix_left = np.array([[1271.71,-0.1385,376.5282],[0.,1270.87,258.1373],[0.,0.,1.]])
        self.distortion_l = np.array([[-0.5688,5.9214,-0.00038018,-0.00052731,-61.7538]])
        # 右相机内参 及 畸变系数
        self.cam_matrix_right = np.array([[1269.2524,-2.098026,367.9874],[0.,1267.1973,246.2712],[0.,0.,1.]])
        self.distortion_r = np.array([[-0.5176,2.4704,-0.0011,0.0012,-16.1715]])
        # 右边相机相对于左边相机的 旋转矩阵 R ， 平移矩阵 T
        self.R = np.array([[1.0000,-0.0088,-0.0043],[0.0088,0.9999,0.0072],[0.0042,-0.0072,1.0000]])
        self.T = np.array([[-68.5321],[-0.5832],[-4.0933]])
        # 焦距
        self.focal_length = 6.00
        # 左右相机之间的距离 取 T 向量的第一维数值 单位 mm
        self.baseline = np.abs(self.T[0])
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

测试函数

# Test.py
from Triangulation import triangulation
from Calib import USB_Camera
import cv2
import numpy as np
# 标定摄像头，config类保存参数
config = USB_Camera()
Roi_points_right = [
[164.0, 634.0],
[257.0, 602.0],
[351.0, 524.0],
[413.0, 446.0],
[460.0, 383.0],
[117.0, 430.0],
[117.0, 336.0],
[117.0, 273.0],
[117.0, 211.0],
[179.0, 414.0],
[194.0, 273.0],
[210.0, 195.0],
[226.0, 148.0],
[241.0, 399.0],
[272.0, 258.0],
[289.0, 180.0],
[319.0, 133.0],
[304.0, 414.0],
[350.0, 304.0],
[367.0, 242.0],
[397.0, 180.0]
]
Roi_points_left = [
[351.0, 649.0],
[445.0, 618.0],
[538.0, 555.0],
[600.0, 477.0],
[647.0, 399.0],
[304.0, 446.0],
[288.0, 351.0],
[288.0, 289.0],
[289.0, 242.0],
[366.0, 430.0],
[366.0, 289.0],
[382.0, 226.0],
[398.0, 164.0],
[414.0, 414.0],
[444.0, 273.0],
[460.0, 211.0],
[476.0, 148.0],
[476.0, 430.0],
[523.0, 320.0],
[553.0, 258.0],
[569.0, 211.0]
]
pts_3d = triangulation(Roi_points_left,Roi_points_right,config.R,config.T,config.cam_matrix_left,config.cam_matrix_right,21)
pts_3d_norm=np.linalg.norm(pts_3d, axis=1, keepdims=True)

for i in range(21):
  dist = pts_3d_norm[i][0]/1000
  print("distance of point {} is {:.4f}m".format(i,dist))

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

实际运用过程中需要修改两个地方
1、把相机参数换成自己的
2、把像素坐标点Roi_points_left 和 Roi_points_right 修改为自己的左右图像素坐标

提示：可以先运行Test.py 看一下demo是否有Bug~
本系列全部代码，请移步 https://github.com/hitsz-zuoqi/Binoculars-tutorial
如果你感觉有用，star for me 是对我最大的鼓励