基于普鲁克分析对两组相机/三维点（已知对应关系）进行相似变换对齐的方法及python代码

  在两组相机（或三维点）对齐方法介绍及实现代码（求解相似变换，包含旋转R、平移t、尺度s）博客中，也介绍了一种求解两组相机之间的相似变换的方法和代码。相比之下，本博客方法本质上是对两组三维点进行变换的，不涉及对两组相机的旋转之间的对齐计算，适用于预测场景不够准确的情况（更一般）。
  对两组相机进行对齐，需要首先明确相机坐标系的定义方式，有两种：

X_world = RX_camera + t
X_camera = RX_world + t

  这两种坐标系的定义是不一样的（其实它们就是一个互逆变换的过程），弄错了的话就没法获得正确的转换结果了（关于这两种坐标系的转换关系，这篇博客里有说明）。在明确了坐标系定义之后，就可以进行计算了。
  转换代码参考BARF论文github源码，链接。下面给出两种坐标系下的相机组对齐方法代码。
  

1 世界坐标系定义

  如果你的坐标系是按照如下方式进行定义的：

X_world = RX_camera + t

  那么直接将需要对齐的R和t传入下面代码中的align_cameras(device, R_pre, t_pre, t_gt)函数中即可。该函数进行的变换是将预测相机组对齐到真值相机组上，需要输出预测相机组的旋转和平移量，以及真值平移量，不需要用到真值旋转。如果要将真值对齐到预测值上，那么反着传递参数就可以了。

# 以下张量类型均为torch.tensor
import torch

# 普鲁克分析，输入的X0和X1分别是两组相机的位置坐标，大小均为[N,3]
def procrustes_analysis(X0, X1):
    # translation
    t0 = X0.mean(dim=0,keepdim=True)
    t1 = X1.mean(dim=0,keepdim=True)
    X0c = X0-t0
    X1c = X1-t1
    # scale
    s0 = (X0c**2).sum(dim=-1).mean().sqrt()
    s1 = (X1c**2).sum(dim=-1).mean().sqrt()
    X0cs = X0c/s0
    X1cs = X1c/s1
    # rotation (use double for SVD, float loses precision)
    U,S,V = (X0cs.t()@X1cs).double().svd(some=True)
    R = (U@V.t()).float()
    # if R.det()<0: R[2] *= -1
    
    # align X1 to X0: X1to0 = (X1-t1)/s1@R.t()*s0+t0
    return t0[0], t1[0], s0, s1, R

# 对齐相机，输入分别为：
# R_pre: [N,3,3]        预测的相机旋转
# t_pre: [N,3]          预测的相机位置
# t_gt:  [N,3]          相机位置真值
# 返回值为：
# R_aligned: [N,3,3]    对齐后的相机旋转
# t_aligned: [N,3]      对齐后的相机位置
@torch.no_grad()
def align_cameras(device, R_pre, t_pre, t_gt):
    # compute 3D similarity transform via Procrustes analysis
    try:
        t0, t1, s0, s1, R = procrustes_analysis(t_gt,t_pre)
    except:
        print("warning: SVD did not converge...")
        t0, t1, s0, s1, R = 0, 0, 1, 1, torch.eye(3,device=device)
    
    # align the camera poses
    R_aligned = R @ R_pre
    t_aligned = (t_pre-t1)/s1@R.t()*s0+t0
    
    return R_aligned, t_aligned
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

  

2 相机坐标系定义

  如果你的坐标系是按照如下方式进行定义的：

X_camera = RX_world + t

那么这里首先会需要将相机坐标系表示转换为世界坐标系的表示方式，然后才能进行对齐，而对齐后也要将结果给再次变换回相机坐标系。这里的变换前后的过程都包含在如下代码的函数中了（所以如果对比来看，可以发现上面的代码是下面代码的简化），传递的时候也只要传R和t即可（不过这里是使用pose的形式，pose=(R|t)，与BARF论文github源码一致）。

import torch

# 求pose的逆，即R'=R.T,  t'=-R.T@t
# pose: [N,3,4]
# pose_inv: [N,3,4]
def invert(self,pose,use_inverse=False):
        # invert a camera pose
        R,t = pose[...,:3],pose[...,3:]
        R_inv = R.inverse() if use_inverse else R.transpose(-1,-2)
        t_inv = (-R_inv@t)[...,0]
        pose_inv = self(R=R_inv,t=t_inv)
        return pose_inv

# 将坐标转换为齐次坐标
# X: [N,3]
# X_hom: [N,4]
def to_hom(X):
    # get homogeneous coordinates of the input
    X_hom = torch.cat([X,torch.ones_like(X[...,:1])],dim=-1)
    return X_hom

# 将坐标X变换到世界坐标系中
# X: [N,4]
# pose: [N,3,4]
# output: [N,3]
def cam2world(X,pose):
    X_hom = to_hom(X)
    pose_inv = invert(pose)
    return X_hom@pose_inv.transpose(-1,-2)    # transpose之#pic_center后pose维度为[N,4,3]

# 普鲁克分析，输入的X0和X1分别是两组相机的位置坐标，大小均为[N,3]
def procrustes_analysis(X0, X1):
    # translation
    t0 = X0.mean(dim=0,keepdim=True)
    t1 = X1.mean(dim=0,keepdim=True)
    X0c = X0-t0
    X1c = X1-t1
    # scale
    s0 = (X0c**2).sum(dim=-1).mean().sqrt()
    s1 = (X1c**2).sum(dim=-1).mean().sqrt()
    X0cs = X0c/s0
    X1cs = X1c/s1
    # rotation (use double for SVD, float loses precision)
    U,S,V = (X0cs.t()@X1cs).double().svd(some=True)
    R = (U@V.t()).float()
    # if R.det()<0: R[2] *= -1
    # align X1 to X0: X1to0 = (X1-t1)/s1@R.t()*s0+t0
    return t0, t1,s0, s1, R

# 对齐相机，输入分别为：
# pose: [N,3,4]
# pose_GT: [N,3,4]
# 返回值为：
# R_aligned: [N,3,3]    对齐后的相机旋转
# t_aligned: [N,3]      对齐后的相机位置
@torch.no_grad()
def align_cameras(device, pose, pose_GT):
    # compute 3D similarity transform via Procrustes analysis
    center = torch.zeros(1,1,3,device=device)
    center_pred = cam2world(center,pose)[:,0] # [N,3]
    center_GT = cam2world(center,pose_GT)[:,0] # [N,3]
    try:
        t0, t1,s0, s1, R = procrustes_analysis(center_GT,center_pred)
    except:
        print("warning: SVD did not converge...")
        t0, t1,s0, s1, R = 0, 0, 1, 1, torch.eye(3,device=device)
    # align the camera poses
    center_aligned = (center_pred-t1)/s1@R.t()*s0+t0
    R_aligned = pose[...,:3]@R.t()
    t_aligned = (-R_aligned@center_aligned[...,None])[...,0]
    return R_aligned, t_aligned
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
61
62
63
64
65
66
67
68
69
70
71

  

3 一点问题

  可以看到，在上面的普鲁克分析函数中，我都有注释掉一行代码：if R.det()<0: R[2] *= -1。这行在BARF源码中是没被注释掉的。其作用是，如果计算出来的R的行列式小于0，那么就将R的最后一行变号，这样的话，R的行列式就会变成正的了。因为一个矩阵是旋转矩阵，当且仅当它是正交矩阵并且它的行列式是单位一。正交矩阵的行列式是 ±1；如果行列式是 −1，则它包含了一个反射而不是真旋转矩阵。而根据Least-Squares Fitting of Two 3-D Point Sets论文所述，如果SVD求出的行列式为负，则表明求解失败了。所以，至于为和源代码里直接将R的最后一行变号，其依据是什么，这样变号会不会对变化结果造成影响，我也还不太清楚。
  但是对此，我有进行了测试。下面称注释掉该行代码的代码为ours，没注释掉该行代码的代码为authors。测试方式是，对于一组相机数据（下面称为真值），给它施加一个变换，包括尺度s、反射矩阵R和平移t，然后使用上述代码将变换结果（下面称为预测值）对齐到真值上。那么，我的预期结果应该是，我获得的对齐后的R和t，应该与真值是一样的。
  首先放上真值的前3个相机的位姿数据（总共有11个），如下所示：

# 真值
R_gt = [[[ 0.9478999   0.0663481   0.31158274]
         [-0.05837532  0.9976859  -0.03485631]
         [-0.31317437  0.01485154  0.9495795 ]]

        [[ 0.98072857  0.02073414  0.19427185]
         [-0.0199321   0.9997828  -0.00608246]
         [-0.19435579  0.00209296  0.9809289 ]]

        [[ 0.9965836   0.00112438  0.08258283]
         [-0.00402177  0.9993818   0.03492666]
         [-0.0824925  -0.03513947  0.995972  ]]]

t_gt = [[-6.562079    0.10006689  0.26979625]
        [-4.7289352  -0.17322822 -0.92882603]
        [-3.3403363  -0.31251827 -1.5564978 ]]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

  然后，给它施加一个包含反射变换的变换，从而获得预测值：

# 变换的R、t、s的值，将这里的R第三行变号就是一个旋转矩阵了
R = [[0.989949703217, -0.135594248772, -0.040172927082],
     [0.132579147816, 0.988693416119, -0.070058442652],
     [-0.049218207598, -0.064028255641, -0.996733665466]]
t =  [11.0, 21.0, -18.0]
s = 10.0

# 预测值
R_pr = [[[ 0.9588697  -0.07019582  0.27503017]
         [ 0.08989697  0.9941614  -0.05967889]
         [ 0.26923516 -0.08194865 -0.9595816 ]]

        [[ 0.9813824  -0.11512312  0.15373732]
         [ 0.1239337   0.99108094 -0.04897964]
         [ 0.14672747 -0.06712096 -0.9868971 ]]

        [[ 0.9904269  -0.13298568  0.03700589]
         [ 0.1339292   0.99069303 -0.02429573]
         [ 0.0334305  -0.02901932 -0.9990197 ]]]

t_pr = [[-54.205353   13.100391  -17.523483 ]
        [-35.206055   13.668443   -6.303665 ]
        [-21.0186     14.5720215  -0.6417084]]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

  下面是ours的结果和authors的结果：

# ours
R_ou = [[[ 0.9479,  0.0663,  0.3116],
         [-0.0584,  0.9977, -0.0349],
         [-0.3132,  0.0149,  0.9496]],

        [[ 0.9807,  0.0207,  0.1943],
         [-0.0199,  0.9998, -0.0061],
         [-0.1944,  0.0021,  0.9809]],

        [[ 0.9966,  0.0011,  0.0826],
         [-0.0040,  0.9994,  0.0349],
         [-0.0825, -0.0351,  0.9960]]])
         
t_ou = [[-6.5621,  0.1001,  0.2698],
        [-4.7289, -0.1732, -0.9288],
        [-3.3403, -0.3125, -1.5565]])

# authors
R_au = [[[ 0.9479,  0.0663,  0.3116],
         [-0.0584,  0.9977, -0.0349],
         [ 0.3132, -0.0149, -0.9496]],

        [[ 0.9807,  0.0207,  0.1943],
         [-0.0199,  0.9998, -0.0061],
         [ 0.1944, -0.0021, -0.9809]],

        [[ 0.9966,  0.0011,  0.0826],
         [-0.0040,  0.9994,  0.0349],
         [ 0.0825,  0.0351, -0.9960]]]
         
t_au = [[-6.5621,  0.1001,  0.3235],
        [-4.7289, -0.1732,  1.5221],
        [-3.3403, -0.3125,  2.1498]]
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

  将ours和authors两个结果与真值进行对比，可以看出，普鲁克分析正确求解出了真值和预测值之间的反射变换，并且将预测值正确对齐到真值上。但是，authors在使用普鲁克分析求得一个反射变换矩阵后，将其第三行进行变号，从而导致计算出的对齐后的R的第三行变号，t的z坐标值计算错误。将正确对齐的结果与错误对齐的结果的t进行可视化，如下图所示。其中红色点是真值，白色点是对齐后的预测值。