(01)ORB-SLAM2源码无死角解析-(45) EPnP 源代码分析(5)→ICP 求解相机位姿

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

在前面的博客 (01)ORB-SLAM2源码无死角解析-(37) EPnP 算法原理详解→理论基础一:控制点选取、透视投影约束 的(09)式，如下(记3D点在世界坐标系下的坐标及对应相机坐标系下的坐标分别是 ${\mathbf{p}}_i^w,{\mathbf{p}}_i^c,i=1,\dots ,n$)：
$$\begin{array}{rl}\left[\begin{array}{c}{\mathbf{p}}_i^c\\ 1\end{array}\right]& ={\mathbf{T}}_{cw}\left[\begin{array}{c}{\mathbf{p}}_i^w\\ 1\end{array}\right]\\ & ={\mathbf{T}}_{cw}\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w\\ \\ {\sum }_{j=1}^4{\alpha }_{ij}\end{array}\right]\\ & =\sum _{j=1}^4{\alpha }_{ij}{\mathbf{T}}_{cw}\left[\begin{array}{c}{\mathbf{c}}_j^w\\ 1\end{array}\right]\\ & =\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{\mathbf{c}}_j^c\\ 1\end{array}\right]\end{array}\text{\hspace{1em}(01)}$$ 可以很明显的知道 3D点 在相机坐标系下的坐标 ${\mathbf{p}}_i^c$ 可以使用相机坐标系下的四个控制点 ${\mathbf{c}}_j^c$ 进行表示。宁外 3D点 在世界坐标下的坐标 ${\mathbf{p}}_i^w$ 可以使用相机坐标系下的四个控制点 ${\mathbf{c}}_j^w$ 进行表示。这里注意一个点，他们的坐标${\alpha }_{ij}$ 是一样的。

可以简单的理解成: 保持坐标不变，进行基底变换，其由 ${\mathbf{c}}_j^w$ 构建的基底$\mathbf{A}$ 到 ${\mathbf{c}}_c^w$ 的构建基底$\mathbf{B}$ 的变换矩阵就是要求解的相机位姿，具体推导过程如下。

 

二、理论推导

世界坐标系的控制点，与世界坐标系的控制点表示如下(注意，此时还没有去质心，或者说去中心化)
$$\begin{array}{l}{\mathbf{p}}_i^w(i=1,2,\cdots ,n)\\ {\mathbf{p}}_i^c(i=1,2,\cdots ,n)\end{array}\text{\hspace{1em}(02)}$$

$(1):$ 首先分别计算它们的质心：
$$\begin{array}{rl}{\mathbf{p}}_0^w& =\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_i^w\\ {\mathbf{p}}_0^c& =\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_i^c\end{array}\text{\hspace{1em}(03)}$$
 
$(2):$ 核心关键是使得关于 $\mathbf{R},\mathbf{t}$ 的误差函数 $F({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})$ 最小(注意，下面的)：
$$F({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n\Vert {\mathbf{p}}_i^c-(\mathbf{R}{\mathbf{p}}_i^w+\mathbf{t}){\Vert }^2\text{\hspace{1em}(04)}$$
  $(3):$ 下面引入质心 ${\mathbf{p}}_0^w$，${\mathbf{p}}_0^c$； 由于核心公式里 ${\mathbf{p}}_i^w$ 前乘了 $\mathbf{R}$，所以 ${\mathbf{p}}_0^w$ 前也乘 $\mathbf{R}$ 才好合并，下式小括号中的值为 0:$$F({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n{\Vert {\mathbf{p}}_i^c-\mathbf{R}{\mathbf{p}}_i^w-\mathbf{t}-({\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w-{\mathbf{p}}_0^c+\mathbf{R}{\mathbf{p}}_0^w)\Vert }^2\text{\hspace{1em}(05)}$$整理得:$$F({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n{\Vert {\mathbf{p}}_i^c-{\mathbf{p}}_0^c-\mathbf{R}({\mathbf{p}}_i^w-{\mathbf{p}}_0^w)+({\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w-\mathbf{t})\Vert }^2\text{\hspace{1em}(06)}$$
 
$(4):$ 对上式进行展开：$$F({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n\left[{\Vert {\mathbf{p}}_i^c-{\mathbf{p}}_0^c-\mathbf{R}\left({\mathbf{p}}_i^w-{\mathbf{p}}_0^w\right)\Vert }^2+{\Vert {\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w-\mathbf{t}\Vert }^2+2{\left({\mathbf{p}}_i^c-{\mathbf{p}}_0^c-\mathbf{R}\left(p_i^w-{\mathbf{p}}_0^w\right)\right)}^T\left({\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w-\mathbf{t}\right)\right]\text{\hspace{1em}(07)}$$
上式子也等价如下形式:
$$\begin{gathered} F({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n{\Vert {\mathbf{p}}_i^c-{\mathbf{p}}_0^c-\mathbf{R}\left({\mathbf{p}}_i^w-{\mathbf{p}}_0^w\right)\Vert }^2 \\ +\frac{1}{n}\sum _{i=1}^n{\Vert {\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w-\mathbf{t}\Vert }^2 \\ +\frac{1}{n}\sum _{i=1}^{n}2{\left({\mathbf{p}}_i^c-{\mathbf{p}}_0^c-\mathbf{R}\left(p_i^w-{\mathbf{p}}_0^w\right)\right)}^T\left({\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w-\mathbf{t}\right)\text{\hspace{1em}(08)} \end{gathered}$$从上面可以看到，函数 $F({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})$ 可以被表示成三段子函数。首先可以看出第3段函数为0，另外第一段函数只与 ${\mathbf{R}}_{cw}$ 相关，第2段函数对于任意的 ${\mathbf{R}}_{cw}$，总能得到 $\mathbf{t}={\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w$，使得其等于0。那么原核型公式可以转化为:
$$minF({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n{\Vert {\mathbf{p}}_i^c-{\mathbf{p}}_0^c-\mathbf{R}\left({\mathbf{p}}_i^w-{\mathbf{p}}_0^w\right)\Vert }^2\text{\hspace{1em}(09)}$$
 
$(5):$观察上式，可以把 ${\mathbf{p}}_i^w-{\mathbf{p}}_0^w$ 与 ${\mathbf{p}}_i^c-{\mathbf{p}}_0^c$ 进行如下表示，也称去质心或者去中心化：
$$\begin{array}{l}{{\mathbf{p}}^{\prime }}_i^w={\mathbf{p}}_i^w-{\mathbf{p}}_0^w(i=1,2,\cdots ,n)\\ {{\mathbf{p}}^{\prime }}_i^c={\mathbf{p}}_i^c-{\mathbf{p}}_0^c(i=1,2,\cdots ,n)\end{array}\text{\hspace{1em}(10)}$$带入之后，可以写成如下形式:
$$minF({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n{\Vert {{\mathbf{p}}^{\prime }}_i^c-\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w\Vert }^2\text{\hspace{1em}(11)}$$
 
$(6):$对上式子进行展开，可以得到如下等式
$$minF({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n[({{\mathbf{p}}^{\prime }}_i^c{)}^T{{\mathbf{p}}^{\prime }}_i^c+({{\mathbf{p}}^{\prime }}_i^w{)}^T{\mathbf{R}}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w-2({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w]\text{\hspace{1em}(12)}$$中间项的 ${\mathbf{R}}^T\mathbf{R}=\mathbf{E}$，也就是没有 $\mathbf{R}$ 了，无 $\mathbf{R}$ 的往后放，写成如下形式:
$$minF({\mathbf{R}}_{cw},{\mathbf{t}}_{cw})=\frac{1}{n}\sum _{i=1}^n[-2({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w]+\frac{1}{n}\sum _{i=1}^n[({{\mathbf{p}}^{\prime }}_i^c{)}^T{{\mathbf{p}}^{\prime }}_i^c+({{\mathbf{p}}^{\prime }}_i^w{)}^T{\mathbf{R}}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w]\text{\hspace{1em}(13)}$$明显的的可以知道上式中右边的子式与 $\mathbf{R}$ 无关，所以求第一项的最小值即可，等价于求 $({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w的$$最大值$ 。
 
$(7):$对 $({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w$ 展开得到如下

$$({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w=\left[\begin{array}{ccc}{p^{\prime }}_{i0}^c& {p^{\prime }}_{i0}^c& {p^{\prime }}_{i0}^c\end{array}\right]\left[\begin{array}{c}{\mathbf{R}}_0\\ {\mathbf{R}}_1\\ {\mathbf{R}}_2\end{array}\right]{{\mathbf{p}}^{\prime }}_i^w\text{\hspace{1em}(14)}$$ 进一步写成如下形式:$$={p^{\prime }}_{i0}^c{\mathbf{R}}_0{{\mathbf{p}}^{\prime }}_i^w+{p^{\prime }}_{i1}^c{\mathbf{R}}_1{{\mathbf{p}}^{\prime }}_i^w+{p^{\prime }}_{i2}^c{\mathbf{R}}_2{{\mathbf{p}}^{\prime }}_i^w={\mathbf{R}}_0{p^{\prime }}_{i0}^c{{\mathbf{p}}^{\prime }}_i^w+{\mathbf{R}}_1{p^{\prime }}_{i1}^c{{\mathbf{p}}^{\prime }}_i^w+{\mathbf{R}}_2{p^{\prime }}_{i2}^c{{\mathbf{p}}^{\prime }}_i^w\text{\hspace{1em}(15)}$$
 
$(8):$下面我们还需要进行如下推导$$\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w({{\mathbf{p}}^{\prime }}_i^c{)}^T=\left[\begin{array}{c}{\mathbf{R}}_0\\ {\mathbf{R}}_1\\ {\mathbf{R}}_2\end{array}\right]\left[\begin{array}{ccc}{p^{\prime }}_{i0}^c{p^{\prime }}_i^w& {p^{\prime }}_{i0}^c{p^{\prime }}_i^w& {p^{\prime }}_{i0}^c{p^{\prime }}_i^w\end{array}\right]\text{\hspace{1em}(16)}$$再进行简化$$=\left[\begin{array}{ccc}{\mathbf{R}}_0{p^{\prime }}_{i0}^c{p^{\prime }}_i^w& {\mathbf{R}}_0{p^{\prime }}_{i1}^c{p^{\prime }}_i^w& {\mathbf{R}}_0{p^{\prime }}_{i2}^c{p^{\prime }}_i^w\\ {\mathbf{R}}_1{p^{\prime }}_{i0}^c{p^{\prime }}_i^w& {\mathbf{R}}_1{p^{\prime }}_{i1}^c{p^{\prime }}_i^w& {\mathbf{R}}_1{p^{\prime }}_{i2}^c{p^{\prime }}_i^w\\ {\mathbf{R}}_2{p^{\prime }}_{i0}^c{p^{\prime }}_i^w& {\mathbf{R}}_2{p^{\prime }}_{i1}^c{p^{\prime }}_i^w& {\mathbf{R}}_2{p^{\prime }}_{i2}^c{p^{\prime }}_i^w\end{array}\right]\text{\hspace{1em}(17)}$$

$(9):$从上面可以观察到，式(15)红色部分为式(17)矩阵对角线元素之和，那么可以得到：$$({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w=Trace(\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w({{\mathbf{p}}^{\prime }}_i^c{)}^T)\text{\hspace{1em}(18)}$$到现在核心公式转为了求下面的式子$$\begin{gathered} max\sum _{i=1}^n({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w=maxTrace(\mathbf{R}\sum _{i=1}^n{{\mathbf{p}}^{\prime }}_i^w({{\mathbf{p}}^{\prime }}_i^c{)}^T) \\ 令:\mathbf{H}={{\mathbf{p}}^{\prime }}_i^w({{\mathbf{p}}^{\prime }}_i^c{)}^T \\ max\sum _{i=1}^n({{\mathbf{p}}^{\prime }}_i^c{)}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_i^w=maxTrace(\mathbf{RH})\text{\hspace{1em}(19)} \end{gathered}$$

$(10):$ 现在对 $\mathbf{H}$ 进行 SVD奇异值分解，即 $$\begin{gathered} \mathbf{H}=\mathbf{U}\Lambda {\mathbf{V}}^T令\mathbf{X}=\mathbf{V}{\mathbf{U}}^T \\ \mathbf{XH}=\mathbf{V}{\mathbf{U}}^T\mathbf{U\Lambda }{\mathbf{V}}^T=\mathbf{V\Lambda }{\mathbf{V}}^T\text{\hspace{1em}(20)} \end{gathered}$$其中 $\mathbf{XH}$ 为正定对称矩阵，所以：
$$Trace(\mathbf{RH})\le Trace(\mathbf{XH})\text{\hspace{1em}(21)}$$ 当 $\mathbf{R}=\mathbf{X}$ 时，$\mathbf{RH}$ 的值最大，所以 $\mathbf{R}$ 的解为(结合前面(08)式的推导，可以求解出$\mathbf{t}$)：$$\mathbf{R}=\mathbf{X}=\mathbf{V}{\mathbf{U}}^T\text{\hspace{1em}(22)}$$

 
$总结:$ 通过前面的一系列推导，总结如下：$$\begin{gathered} \mathbf{H}={{\mathbf{p}}^{\prime }}_i^w({{\mathbf{p}}^{\prime }}_i^c{)}^T=\mathbf{U}\Lambda {\mathbf{V}}^T \\ \mathbf{R}=\mathbf{V}{\mathbf{U}}^T \\ \mathbf{t}={\mathbf{p}}_0^c-\mathbf{R}{\mathbf{p}}_0^w\text{\hspace{1em}(23)} \end{gathered}$$

 

三、代码注释

代码位于 src/PnPsolver.cc 文件中，主调函数如下：

/**
 * @brief 根据已经得到的控制点在当前相机坐标系下的坐标来恢复出相机的位姿
 * @param[in]  ut         vi
 * @param[in]  betas      betas
 * @param[out] R          计算得到的相机旋转R
 * @param[out] t          计算得到的相机位置t
 * @return double         使用这个位姿,所得到的重投影误差
 */
double PnPsolver::compute_R_and_t(const double * ut, const double * betas,
			     double R[3][3], double t[3])
{
  // Step 1 根据前面的计算结果来"组装"得到控制点在当前相机坐标系下的坐标
  compute_ccs(betas, ut);
  // Step 2 将世界坐标系下的3D点的坐标转换到控制点的坐标系下
  compute_pcs();
  // Step 3 调整点坐标的符号,来保证在相机坐标系下点的深度为正
  solve_for_sign();

  // Step 4 ICP计算R和t 
  estimate_R_and_t(R, t);

  // Step 5 计算使用这个位姿,所得到的每对点平均的重投影误差,作为返回值
  return reprojection_error(R, t);
}

estimate_R_and_t 的注释如下：

/**
 * @brief 用3D点在世界坐标系和相机坐标系下对应的坐标，用ICP求取R t
 * @param[out] R   旋转
 * @param[out] t   平移
 */
void PnPsolver::estimate_R_and_t(double R[3][3], double t[3])
{
  // Step 1 计算3D点的质心
  double pc0[3],                                              //3D点世界坐标系下坐标的质心
         pw0[3];                                              //3D点相机坐标系下坐标的质心
  // 初始化这两个质心
  pc0[0] = pc0[1] = pc0[2] = 0.0;
  pw0[0] = pw0[1] = pw0[2] = 0.0;

  // 然后累加求质心
  for(int i = 0; i < number_of_correspondences; i++) {
    const double * pc = pcs + 3 * i;
    const double * pw = pws + 3 * i;

    for(int j = 0; j < 3; j++) {
      pc0[j] += pc[j];
      pw0[j] += pw[j];
    }
  }
  for(int j = 0; j < 3; j++) {
    pc0[j] /= number_of_correspondences;
    pw0[j] /= number_of_correspondences;
  }

  // 准备构造矩阵A,B以及B^T*A的SVD分解的值
  double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
  CvMat ABt   = cvMat(3, 3, CV_64F, abt);       // H=B^T*A
  CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);     // 奇异值分解得到的特征值
  CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);     // 奇异值分解得到的左特征矩阵
  CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);     // 奇异值分解得到的右特征矩阵

  // Step 2 构造矩阵H=B^T*A,不过这里是隐含的构造
  cvSetZero(&ABt);
  // 遍历每一个3D点
  for(int i = 0; i < number_of_correspondences; i++) {
    // 定位
    double * pc = pcs + 3 * i;
    double * pw = pws + 3 * i;

    // 计算H=B^T*A,其中的两个矩阵构造和相乘的操作被融合在一起了
    for(int j = 0; j < 3; j++) {
      abt[3 * j    ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
      abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
      abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
    }
  }

  // Step 3 对得到的H矩阵进行奇异值分解
  cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);

  // Step 4 R=U*V^T, 并且进行合法性检查
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 3; j++)
      R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
  
  // 注意在得到了R以后,需要保证 det(R)=1>0
  const double det =
    R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
    R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
  // 如果小于0那么就要加负号
  if (det < 0) {
    R[2][0] = -R[2][0];
    R[2][1] = -R[2][1];
    R[2][2] = -R[2][2];
  }

  // Step 5 根据R计算t
  t[0] = pc0[0] - dot(R[0], pw0);
  t[1] = pc0[1] - dot(R[1], pw0);
  t[2] = pc0[2] - dot(R[2], pw0);
}

 

四、结语

其上的的重投影误差函数再前面已经讲解过，所以这里就不做重复分析了。通过这一系列的博客，可以说是把 EPnP 介绍的非常全面了。主要流程为 控制点选取、通过透视投影约束求得beta初始解、高斯牛顿对beta进行优化(QR分解求增量)、求解相机位姿。那么朋友就下篇博客见了→终于可以学习新的知识了。
 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件