相机对齐

问题背景

在多目立体视觉中，不同的 dense reconstruction 算法提取出来的相机位姿不会完全一样，如果要对这多种 dense reconstruction 算法输出的点云/相机位姿进行对齐。

问题描述

已知一系列相机在世界坐标系A下的旋转矩阵和位移向量为$[R_r|t_r]$、在世界坐标系B下的旋转矩阵和位移向量为$[R|t]$，求坐标系B到坐标系A的变换矩阵$(s,[R_t|t_t])$（$s$为一对角矩阵表示缩放）使得坐标系B下的任意坐标$P$映射为坐标系A下的坐标$P_r$：$P_r=s(R_tP+t_t)$。

求解方法1

显然，同一个点世界坐标系AB下的坐标$P_r$和$P$在同一个相机坐标系下的坐标必然成比例：

$RP+t=s_r(R_rP_r+t_r)$

带入$P_r=s(R_tP+t_t)$：

$\begin{aligned} RP+t&=Q\\ P&=R^{-1}Q-R^{-1}t\\ \end{aligned}$

$\begin{aligned} RP+t&=s_rR_rs(R_tP+t_t)+s_rt_r\\ RP+t&=ss_rR_rR_tP+ss_rR_rt_t+s_rt_r \end{aligned}$

若带入$P=\bm 0$则有：

$\begin{aligned} t&=ss_rR_rt_t+s_rt_r\\ ss_rR_rt_t&=t-s_rt_r\\ t_t&=\frac{1}{ss_r}R_r^{-1}(t-s_rt_r)\\ \end{aligned}$

进而方程可消去位移项：

$\begin{aligned} RP&=ss_rR_rR_tP\\ P&=ss_rR^{-1}R_rR_tP \end{aligned}$

显然要让上式成立，$ss_rR^{-1}R_rR_t$必为单位矩阵：

$\begin{aligned} ss_rR^{-1}R_rR_t&=I\\ ss_rR_t&=R_r^{-1}R\\ \end{aligned}$

其中$R_t$、$R_r$和$R$都是旋转矩阵，所以必有$ss_r=1$而$R_t=R_r^{-1}R$，再带入$t_t$的计算式易得：

$t_t=R_r^{-1}(t-\frac{1}{s}t_r)$

仍有未知量需要求解，如何求解$t_t$和$s$？ 多个相机的$R_{r,i}$和$t_{r,i}$可构成方程组求解之。

由于$t_t$对于所有相机都相同，可以取任意两相机$i,j$求$s$：

$\begin{aligned} R_{r,i}^{-1}(t_i-\frac{1}{s}t_{r,i})&=R_{r,j}^{-1}(t_j-\frac{1}{s}t_{r,j})=t_t\\ R_{r,i}^{-1}(st_i-t_{r,i})&=R_{r,j}^{-1}(st_j-t_{r,j})\\ sR_{r,i}^{-1}t_i-R_{r,i}^{-1}t_{r,i}&=sR_{r,j}^{-1}t_j-R_{r,j}^{-1}t_{r,j}\\ s(R_{r,i}^{-1}t_i-R_{r,j}^{-1}t_j)&=R_{r,i}^{-1}t_{r,i}-R_{r,j}^{-1}t_{r,j}\\ s&=\frac{R_{r,i}^{-1}t_{r,i}-R_{r,j}^{-1}t_{r,j}}{R_{r,i}^{-1}t_i-R_{r,j}^{-1}t_j}\\ \end{aligned}$

最后将$s$带入公式易得$t_t$。