写文章

EPnP算法

小米
怕什么真理无穷
​ 关注他
4 人 赞同了该文章

参考资料：

  1. 杨小东的知乎 zhuanlan.zhihu.com/p/59
  2. blog.youkuaiyun.com/jessecw79
  3. EPnP: An accurate O(n) solution to the PnP problem
  4. 视觉SLAM十四讲
  5. 柴政pnp算法讲解，youtube.com/watch?

问题模型

已知世界坐标下的 [公式] 个3d点，以以及它们在图像上对应的2d像素点，还有相机的内参数。

求解相机坐标系到世界坐标系的 [公式] .

控制点和HB坐标

三维点的非齐次坐标表示为 ：

[公式]

[公式] 表示点对个数。EPnP引入了控制点,利用控制点的线性组合对3d点进行表示

[公式]

用上标 [公式] 和 [公式] 分别表示世界坐标系和相机坐标系，世界坐标系下的三维点可以用4个控制点表达：

[公式]

控制点选定好之后， [公式] 称为homogeneous barycentric coordinates，就是唯一确定的。将上式写成齐次坐标的形式。

[公式]

上式的含义就是3D点齐次坐标可以表示为控制点齐次坐标的线性组合。假设相机外参数为 [公式] 为，控制点在相机坐标系下的坐标 [公式] 有：

[公式]

对于3D点在相机坐标系下的坐标 [公式] 有：

[公式]

也就是说，同一个3D点在相机坐标系和世界坐标系的HB坐标是相同的，我们可以在世界坐标系下求得HB坐标，拿到相机坐标系使用。进一步如果我们求出了控制点在相机坐标系下的坐标 [公式] ,我们就可以得到世界坐标系下的3D点 [公式] 到相机坐标系下的3D点 [公式] 匹配关系,这样的话，就可以得到相机坐标系和世界坐标系之间3D-3D之间的匹配关系，进一步利用ICP算法进行求解 [公式] 。

控制点选择

世界坐标系下的3D点集， [公式] ,选择3D参考点的中心为第一个参考点：

[公式]


其余的点定义在数据的主方向上，首先定义矩阵 [公式]

[公式]


计算 [公式] 的特征值 [公式] ，以及对应的特征向量 [公式] ,那么剩余的三个控制点为

[公式]

控制点确定后，根据式（4）可以得到每个世界坐标系下的 [公式] 对应的HB坐标。

[公式]

求解控制点在相机坐标系下的坐标 [公式] 

由相机的投影模型。

[公式]


可以进一步有：

[公式]

消去 [公式] ，可以得到两个线性方程

[公式]

其中 [公式] ，相机内部参数， [公式] 为已知量，未知量为控制点在相机坐标系下的坐标 [公式] 。把所有的 [公式] 个点都串联起来，得到线性方程

[公式]

展开有

[公式]

其中未知量 [公式] .对该方程求解可以得到控制点在相机坐标系下的坐标。式(17)的解为M的零空间。

[公式]

其中 [公式] 为 [公式] 的右奇异向量，对应的奇异值为0。具体的求解步骤为：求解 [公式] 的特征值和特征向量，特征值为0的特征向量为 [公式] .

根据约束 [公式] ,求解 [公式] 。其中 [公式] 为世界坐标系下4个控制点之间的距离的平方，可以直接直接计算出来，是一个6维的向量。

直接考虑 当N=4的时候,第i个控制点在相机坐标系下的坐标有

[公式]

控制点之间的长度约束,控制点在世界坐标系和相机坐标系下的长度相等。

[公式]

该约束可以构建方程

[公式]

其中 [公式] , [公式] .

对于

[公式]

近似求解 [公式] ，记 [公式] ，也就是把 [公式] 写成了列向量的形式。

[公式]

  • 近似求解方法1

[公式]

  • 近似求解方法2

[公式]

  • 近似求解方法3

[公式]

可以根据以上三种方法求得， [公式] 的初值

高斯牛顿优化

优化的目标函数

[公式]

残差项

[公式]

优化变量 [公式] ,

残差项对优化变量的雅克比矩阵

[公式]

normal equation:

[公式]

利用Gauss-newton迭代法求得最优的 [公式]

计算相机位姿

  • 计算控制点在相机坐标系下的坐标 ：[公式]
  • 计算3D参考点在相机参考坐标系的坐标。[公式]
  • 计算 [公式] 的重心 [公式] 和矩阵 [公式] [公式]
  • 计算 [公式] 的重心 [公式] 和矩阵 [公式] .[公式]
  • 计算矩阵 [公式]
  • 计算 [公式] 的svd分解 :[公式]
  • 计算旋转 [公式]

[公式]

如果 [公式] ，那么 [公式]

  • 计算位姿中的平移 [公式]

[公式]