week63 PnP位姿估计 基于ArUco Marker来估算

相机位姿估计

相机位姿估计是指给定若干图像，估计其中相机运动的问题。求解方法通常分特征点法和直接法两种，这也是视觉里程计的两类基本方法。这里主要讨论特征点法（间接法）中的3D-2D情况。

特征点法

特征点法的思路，是先从图像当中提取许多特征，然后在图像间进行特征匹配，这样就得到许多匹配好的点，再根据这些点进行相机位姿的求解。相机位姿求解部分则和图像本身关系不大了。比方说下图是ORB特征匹配的结果。

特征点法

特征匹配之后，我们得到了一组配对点，以及它们的像素坐标。剩下的问题是说，怎么用这组配对点去计算相机的运动。这里，根据传感器形式的不同，分成三种情况：

• 单目相机→2D-2D的配对点
• RGBD or 双目相机→3D-3D的配对点
• 一张图中3D信息，另一张图只有2D信息→3D-2D的配对点
  第三种情况可能出现在单目SLAM，我们使用预先的信息计算了3D的地图点，现在又来了一张2D图像，所以会有3D-2D的情况。或者，在RGBD和双目中，也可以忽略其中一张图的3D信息，使用3D-2D的配对点。 无论如何，这三种情况都是现实存在的，所以处理方式也分为三种。在这里针对第三种情况，讨论基于ArUco Marker的3D-2D的相机位姿估计算法。

基于ArUco Marker的3D-2D的相机位姿估计算法

PnP
PnP（Perspective n Points）
我们知道n个点的3D位置和它们在二维图像平面的投影，然后要算相机的位姿。PnP的做法有好多种：直接线性变换，P3P，EPnP，UPnP等等，调包的话直接看OpenCV的SolvePnP中的参数即可，好用的都在那里。除此之外，通常认为线性方程解PnP，在有噪声的情况下表现不佳，所以一般以EPnP之类的解为初始值，构建一个Bundle Adjustment（BA）去做优化。

ArUco Marker

基于二进制的Maeker的主要便利之处在于一个Marker可以提供足够多的（四个角）来获取相机的信息。同时，其内部的二进制编码非常robust，允许错误检测和校正。
一个ArUco marker是一个二进制平方标记，它由一个宽的黑边和一个内部的二进制矩阵组成，内部的矩阵决定了它们的id。黑色的边界有利于快速检测到图像，二进制编码可以验证id，并且允许错误检测和矫正技术的应用。marker的大小决定了内部矩阵的大小。例如，一个4x4的marker由16bits组成。

ArUco Marker

应当注意到，我们需要检测到一个Marker在空间中发生了旋转，但是，检测的过程需要确定它的初始角度，所以每个角落需要是明确的，不能有歧义，保证上述这点也是靠二进制编码完成的。
整个ArUco库主要包含了两部分：src包含了库的本身（一些源码），utils里面则是包含了一些应用的实例

安装过程

mkdir build
cd build
cmake ..
make
sudo make install

核心思路

整体的思路是我们知道相机的内参K和H矩阵，对相机进行三维世界的位姿估计，也就是我们知道地上marker的三维坐标以及它们在图像平面的uv坐标，想要算出Rot和Trans即相机外参

算法流程

• 变量初始化
  定义相机的内参矩阵于畸变参数 定义观测到的特征点的数目等变量
• 图像矫正 获取内参K
  通过相机的参数矩阵及失真参数来对图像的点进行矫正 opencv中使用undistortPoints函数校正特征点（对于针孔相机）
• 构建观测矩阵 求H矩阵
  有num_points个观测点 创建(2 multiply num_points,9)观测矩阵 通过SVD解决Ax=0的问题 求出x即为所需要的H矩阵
• 求解R和T
  K.inverse() multiply H=[h1, h2, h3]
  构建矩阵[h1, h2, h1xh2]做SVD＝USV(transpose)
  则R=U*V(transpose) T=h3/h1(norm)

推荐参考

https://www.zhihu.com/question/51510464
https://blog.csdn.net/gwplovekimi/article/details/115245558