我的项目遇到了一些问题，这个问题伴随了我好长时间一直没有解决，这几天询问了老师后终于有了新的思路，接下来我说明一下我的问题，以及解决方法

问题原因

很普通的问题，我在利用双目测出深度之后，会形成以左相机光心为原点，图片的水平方向为X轴（左小右大），垂直方向为Y轴（上小下大），垂直于相机成像平面为Z轴（远小近大），类型为右手坐标系。但这个坐标系并不能满足于我的项目，我需要强制转换一个坐标系。

一开始我想到的是制作旋转平移矩阵，也就是：

但我忽略了秩的问题，总之直接去求并不能求出来。

解决方法

根据老师的提醒，我还是找到了解决的具体方法，直接采用SVD（奇异值分解）可以求出自己想要的具体矩阵。

奇异值分解

又捡起了线性代数方面的知识，简单回顾了一下奇异值分解具体原理。

奇异值分解是把矩阵分解成了：

其中，U和V是两个方阵，Sigma 矩阵是一个对角线是奇异值、其他位置是0的矩阵。

numpy 提供了奇异值分解，直接调用即可。

1
2
import numpy as np
u, s, vt = np.linalg.svd(M)

SVD法坐标系转换

其实我并没有看懂具体原理是什么，我就是随便找到了公式，但这样也就足够了。

我并没有去构造完整的旋转平移矩阵，我把它拆开了：

简单讲一下：

  • P是新的坐标，P_0是旧坐标，两者的形状都是(N, 3)，第0维度是点的个数，第1维度坐标（三个数字为一个坐标）
  • R是旋转矩阵，T是平移向量

既然使用了奇异值分解，那么就必须去制作被分解的矩阵，这里被分解的矩阵用M表示：

比较麻烦，按上面公式的步骤来讲就是：

  • 求出转换前和转换后的平均坐标。
  • 用转换前和转换后的坐标减去自己的平均坐标。
  • 进行内积操作
  • 累计求和

当然写程序的时候可以使用并行化的原理，这样不仅节省时间，还方便处理。

有了需要被分解的矩阵就可以奇异值分解了：

但我们并不需要奇异值，所以 Sigma 可以不要。

接下来就是合成旋转矩阵：

很简单，就直接矩阵相乘。

注意：我们需要计算一下旋转矩阵的行列式，因为很有可能计算出来一个镜像旋转矩阵，这样是永远不额能旋转成功的。如果旋转矩阵的行列式为正数就正确，为负数就需要让旋转矩阵取反变成正数

所以在

平移矩阵就更好求了，但需要两个点的平均值：

程序

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
def points3D_transform(points1, points2):
    """
    坐标转换
    :param points1: shape: (M, 3)
    :param points2: shape: (N, 3)
    :return:
    """

    # 计算出所有点的平均坐标
    # 转换前和转换后都要计算
    center_points1 = np.mean(points1, 0)
    center_points2 = np.mean(points2, 0)

    # 每个坐标减去平均值
    new_points1 = points1 - center_points1
    new_points2 = points2 - center_points2

    # 矩阵相乘，构造一个矩阵
    M = new_points2.T @ new_points1

    # 使用奇异值分解
    u, s, vt = np.linalg.svd(M)

    # 旋转矩阵
    R = u @ vt

    # 计算出行列式是否是负数
    if np.linalg.det(R) < 0:
        # 小数就反了
        R = -R

    # 反向计算出
    T = center_points2.T - (R @ center_points1)

    return R, T

我随便做了一个矩阵，简单测试一下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# 转换前坐标
point1 = np.array([
    [162.68627451, -13.23529412, 406.],
    [-173.68627451, -13.62745098, 405.05882353],
    [-5.1372549, 71.78431373, 415.11764706],
    [-5.15686275, -96.25490196, 399.15686275]])

# 欧氏距离
x = comput_distance(world_matrix[0], world_matrix[1]) / 2
y = comput_distance(world_matrix[2], world_matrix[3]) / 2

# 转换后坐标
point2 = np.array([
    [x, 0, 0],
    [-x, 0, 0],
    [0, y, 0],
    [0, -y, 0]])

# 转换
R, T = points3D_transform(point1, point2)