PnP 问题
欢迎访问我的博客首页。
PnP 问题
- 1. DLT
- 2. P3P
- 3. G2O 求解 PnP
-
- 3.1 单目
- 3.2 双目
- 4. 自定义顶点与边优化内参
-
- 4.1 二元边
- 4.2 三元边
- 4.3 总结
- 5. 参考
PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。即，知道了 n 个 3D 空间点及其投影位置，求相机的位姿变换。如下图，已知一些点在左边相机坐标系中的三维坐标，和在右边相机坐标系中的像素坐标，求这两个相机坐标系间的位姿变换，就是 PnP 问题。
下面介绍四种求解 PnP 问题的方法。DLT、P3P 和 EPnP 利用若干对匹配点即可求得位姿变换，非线性优化可以利用更多观测值构造最小二乘问题。 所以，可以用前三者求得一个初始值，再用非线性优化在此初始值的基础上迭代调整。由于非线性优化通常不需要迭代很多次，所以实际使用时，可以直接使用非线性优化，在旋转矩阵为单位矩阵、平移向量为零向量的基础上直接迭代调整。
EPnP 的具体原理在这里。
1. DLT
DLT（Direct Linear Transform）即直接线性变换法，用 12 个参数表示增广矩阵 [R|t]，因此需要至少 6 对匹配点就可以求得该增广矩阵。然后使用 QR 分解求得位姿。
2. P3P
P3P 使用 3 对匹配点求得四个解，再使用 1 对验证点得到一个合理的解。
#include 传入 solveP3P 的点对只能是 3 对或 4 对，否则会报错。无论传入的是 3 对点还是 4 对点，函数 solveP3P 都可能得出多解，需要我们自己选择正确的解。
由于我们研究的是 3D-2D 问题，所以使用函数 cam2pix 把一组 3D 点变换成了 2D 点。相机参数可以是任意的，只要在函数 cam2pix 和函数函数 solveP3P 中保持一致。
3. G2O 求解 PnP
使用非线性优化求解 PnP 问题常被称为 BA，因此 G2O 把为 PnP 问题定义的边和顶点存放在 g2o/types/sba。G2O 定义了多个求解 PnP 问题的边，下面介绍常用的 5 个边。
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{...};
class EdgeSE3ProjectXYZ
: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap> {...};
class EdgeSE3ProjectXYZOnlyPose
: public BaseUnaryEdge<2, Vector2D, VertexSE3Expmap> {...};
class EdgeStereoSE3ProjectXYZ
: public BaseBinaryEdge<3, Vector3D, VertexSBAPointXYZ, VertexSE3Expmap> {...};
class EdgeStereoSE3ProjectXYZOnlyPose
: public BaseUnaryEdge<3, Vector3D, VertexSE3Expmap> {...};
第一个边与第二个边都用于单目。它们的区别主要在于设置相机内参的方法不同：第一个边的用法在下面 3.1 节，它不包含相机内参，需要使用 optimizer.addParameter(camera) 设置相机内参；第二个边的用法参考 3.2 节，可以直接为其 fx, fy, cx, cy, bf 成员赋值。
第二个边和第三个边的区别在于是否优化三维点，所以在第二个边中固定三维点就能实现和第三个边一样的作用。第四、第五个边可以用于双目摄像头获取的二维图像，具体用法见 3.2 节。
第二个边与第四个边经常一起使用，第三个边与第五个边经常一起使用。这是因为在双目相机中，有的物方点只在其中一个相机中成像，有的物方点在两个相机中都成像。
3.1 单目
下面是使用 EdgeProjectXYZ2UV 求解 PnP 的例子。该程序只能使用 G2O 20170730 及以前版本，因为 G2O 20200410 及以后的版本定义优化器需要使用智能指针，G2O 20201223 及以后的版本用 VertexPointXYZ 替代了 VertexSBAPointXYZ。
#include 在函数 bundleAdjustment 中，我们先定义了优化器，然后向其中添加顶点、参数、边：
- 添加顶点：第 25 行开始添加一个位姿顶点，编号为 0。第 32 行开始添加 6 个路标顶点，编号从 1 开始。
- 添加参数：第 42 行开始添加一个参数，编号为 0。
- 添加边：第 49 行开始添加 6 个边，编号从 0 开始。
每个边对应一个路标顶点，由于路标顶点编号从 1 开始，边编号从 0 开始，所以在第 54 行使用的是 edge_idx + 1。
第 54 行和第 55 行让一个二元边连接两个顶点：0 号顶点是路标 VertexSBAPointXYZ，1 号顶点是位姿 VertexSE3Expmap。顺序不能颠倒，因为边 EdgeProjectXYZ2UV 的模板参数决定了这个顺序：
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{...};
第 57 行把编号为 params_idx 的参数存储在索引为 0 的参数容器中。0 号顶点会根据参数的编号从优化器中取出该参数：
// OptimizableGraph 是 EdgeProjectXYZ2UV 的基类。
bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId) {
if ((int)_parameters.size() <= argNum) return false;
if (argNum < 0) return false;
*_parameters[argNum] = 0;
_parameterIds[argNum] = paramId;
return true;
}
void EdgeProjectXYZ2UV::computeError() {
// 省略。
const CameraParameters *cam = static_cast<const CameraParameters *>(parameter(0));
// 利用该参数计算投影误差。
}
下面是 CMakeLists.txt 的内容。其中，路径 D:/MinGW/libraries/g2o_20170730 是安装 G2O 的位置，包含以下文件：
bin
include
lib
FindCSparse.cmake
FindG2O.cmake
cmake_minimum_required(VERSION 3.2)
project(test)
add_compile_options(-std=c++14)
set(Eigen3_DIR D:/MinGW/libraries/share/eigen3/cmake)
find_package(Eigen3 REQUIRED)
set(G2O_ROOT D:/MinGW/libraries/g2o_20170730)
set(CMAKE_MODULE_PATH ${G2O_ROOT})
find_package(G2O REQUIRED)
find_package(CSPARSE REQUIRED)
include_directories(
${EIGEN3_INCLUDE_DIRS}
${G2O_INCLUDE_DIR}
${CSPARSE_INCLUDE_DIR}
D:/MinGW/libraries/include
)
link_directories(${G2O_ROOT}/lib)
add_executable(main main.cpp)
target_link_libraries(main
g2o_core
g2o_stuff
g2o_types_sba
g2o_types_slam3d
)
# cmake -G "MinGW Makefiles" ..
程序运行结果如下面的左图。
分析：可以看到计算的结果有一定的误差，尤其是平移向量 (0.19,-0.57,0.02) 与实际值 (0,-1,0) 相差较大。这是因为我们在使用 G2O 优化时同时调整了位姿与 3D 点。vertex 1 到 vertex 6 都被调整了，所以不能得到实际的位姿。但这样做在工程中却是有意义的。因为 3D 点的坐标准确度不那么可信，所以有必要把它们和位姿一块调整。如果能确定这些 3D 点的坐标是准确的，就可以得到准确的位姿，比如去掉对 point->setFixed(true) 的注释，就可以得到右图的结果。
3.2 双目
双目摄像头有视差，根据视差公式可以得到深度信息，所以使用 G2O 定义的边 EdgeStereoSE3ProjectXYZ 可以利用更多约束求解 PnP 问题。
#include 如果有双目相机，视差是可以直接获取的。由于没有双目相机，我们在第 44 行假设基线是 1 米，在第 56 行利用像素点的深度信息生成视差信息。第 77 行是为每个像素点准备的深度信息，用在第 56 行生成视差。
从上面的程序可以看到，双目 PnP 边和单目 PnP 边用法几乎完全一样，只是测量值除了像素坐标 p p p 外，多了一个维度表示 p x p_x px 与视差的差值。
4. 自定义顶点与边优化内参
下面我们将自定义顶点与边，在求解 PnP 的问题的过程中，把相机内参也当作待优化项。我们不考虑桶形畸变和枕形畸变，且只针对单目相机。如果考虑畸变，只需再加两个畸变参数，实现起来很容易。
4.1 二元边
《视觉 SLAM 十四讲 第 2 版》第 9 章的示例程序演示了怎么优化畸变参数，本小节就是参考该实例程序而来。
#include 结构体 SE3Vector4 用于存放自定义顶点 VertexSE3Vector4 的待优化的量：位姿和内参。类似于 g2o 为顶点 VertexSE3Expmap 定义的 SE3Quat。
自定义边 VertexSE3Vector4 时使用的模板参数是 <10, SE3Vector4>，前者指定该边的成员函数 oplusImpl 的参数数组长度，暂且称为增量维度，后者指定该边的成员变量 _estimate 的类型。
配置文件 CMakeLists.txt 内容如下。
cmake_minimum_required(VERSION 3.2)
project(test)
add_compile_options(-std=c++14)
set(Eigen3_DIR D:/MinGW/libraries/share/eigen3/cmake)
find_package(Eigen3 REQUIRED)
set(G2O_ROOT D:/MinGW/libraries/g2o_20170730)
set(CMAKE_MODULE_PATH ${G2O_ROOT})
find_package(G2O REQUIRED)
find_package(CSPARSE REQUIRED)
include_directories(
${EIGEN3_INCLUDE_DIRS}
${G2O_INCLUDE_DIR}
${CSPARSE_INCLUDE_DIR}
D:/MinGW/libraries/Sophus220401/include
D:/MinGW/libraries/fmt800/include
)
link_directories(
${G2O_ROOT}/lib
D:/MinGW/libraries/fmt800/lib
)
add_executable(main main.cpp)
target_link_libraries(main
g2o_core
g2o_stuff
g2o_types_sba
g2o_types_slam3d
g2o_types_sim3
fmt
)
# cmake -G "MinGW Makefiles" ..
4.2 三元边
下面定义三元边实现 4.1 节的功能，边连接的三个顶点是三维路标、位姿、相机内参。
#include 从第 35 行可以看出，定义三元边时继承了 BaseMultiEdge。和定义一元边、二元边不同的是，定义三元边时的模板参数只有增量维度和测量值类型，没有各顶点类型。在 39 行使用 resize 函数指定该边是三元边。
4.3 总结
从上面的两个程序可以看出，在位姿不固定的情况下：如果固定三维的路标顶点，无论相机内参的四个初始值与真实值相差多大，都能收敛到真实值；如果不固定路标顶点，相机内参就难以收敛到真实值，而是在初始值附近。这个原因很好解释：路标、像素坐标、位姿、内参这几项，确定的项越多，优化得到的不确定项就越准确。因此在 SLAM 中，标定相机内参很重要。
求解 PnP 问题的方法有很多，比如本文介绍的 DLT、P3P、EPnP 以及第 3、4 节使用 G2O 的非线性优化。第 3、4 节把空间点变换到相机坐标系，再投影得到像素坐标，优化的目标是使像素坐标与观测值的误差趋向 0，这就是 BA（bundle adjustment）算法。
5. 参考
- PnP，博客园。
- OpenCV 中的 9 种 PnP 算法，CSDN。
- G2O 路标顶点的 setMarginalized 函数，知乎专栏。