基于普鲁克分析对两组相机/三维点（已知对应关系）进行相似变换对齐的方法及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

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

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的最后一行变号，其依据是什么，这样变号会不会对变化结果造成影响，我也还不太清楚。
  我在测试过程中，发现它确实是会对结果造成影响的。如下两张图所示，红色点是真值，白色点是对齐到真值上的预测值。其中上图是使用源代码中若求出的R行列式为负，则将其最后一行变号的这行代码用上的结果，下图则是将其注释掉的结果，也就是第一节第二节所给出的代码。可以看出，上图的结果是错误的，而下图的结果是正确的。