OpenCV实战（24）——相机姿态估计

0. 前言

校准相机后，就可以将捕获的图像与物理世界联系起来。如果物体的 3D 结构是已知的，那么就可以预测物体如何投影到相机的传感器上，图像形成的过程由投影方程描述。当方程的大部分项已知时，就可以通过观察一些图像来推断其他元素 (2D 或 3D) 的值。相机姿态估计就是通过几个已知坐标的特征点，以及这些点在照片中的成像位置，求解出相机位于坐标系内的坐标与旋转角度。在本节中，我们将研究观察已知 3D 结构时的相机姿态估计问题。

1. 相机姿态估计

我们考虑一个简单的对象——公园里的长凳，使用我们在上一节中校准的相机和镜头系统拍摄了以下图像，并在长凳上手动识别了 8 个不同的图像点，用于相机姿态估计 (camera pose estimation)：

通过访问该对象，可以进行许多 3D 物理测量。长凳由一个 242.5cmx53.5cmx9cm 的座椅和一个 242.5cmx24cmx9cm 的靠背组成，靠背固定在座椅上方 12cm 处。使用此信息，我们可以轻松推导出以对象为中心的参考系中 8 个识别点的 3D 坐标(将原点固定在两个平面之间的交点的左端)。

(1) 在算法中要做的第一件事是创建一个包含以上对象坐标的 cv::Point3f 向量：

// 输入图像点
std::vector<cv::Point2f> imagePoints;
imagePoints.push_back(cv::Point2f(207, 60));
imagePoints.push_back(cv::Point2f(670, 103));
imagePoints.push_back(cv::Point2f(637, 240));
imagePoints.push_back(cv::Point2f(183, 156));
imagePoints.push_back(cv::Point2f(185, 179));
imagePoints.push_back(cv::Point2f(633, 273));
imagePoints.push_back(cv::Point2f(541, 332));
imagePoints.push_back(cv::Point2f(73, 213));

(2) 现在的问题是在拍摄以上照片时，相机相对于这些点的位置。由于这些已知点在 2D 图像平面上的图像坐标也是已知的，那么使用 cv::solvePnP 函数就很容易估计相机位置。这里，我们手动建立了 3D 和 2D 点之间的对应关系，但是我们也可以自动获取此信息：

// 输入对象点
std::vector<cv::Point3f> objectPoints;
objectPoints.push_back(cv::Point3f(0, 45, 0));
objectPoints.push_back(cv::Point3f(242.5, 45, 0));
objectPoints.push_back(cv::Point3f(242.5, 21, 0));
objectPoints.push_back(cv::Point3f(0, 21, 0));
objectPoints.push_back(cv::Point3f(0, 9, -9));
objectPoints.push_back(cv::Point3f(242.5, 9, -9));
objectPoints.push_back(cv::Point3f(242.5, 9, 44.5));
objectPoints.push_back(cv::Point3f(0, 9, 44.5));
// 根据 3D/2D 点计算相机姿态
cv::Mat rvec, tvec;
cv::solvePnP(objectPoints, imagePoints,         // 对应的 3D/2D 点
                cameraMatrix, cameraDistCoeffs, // 标定
                rvec, tvec);                    // 输出姿态
cv::Mat rotation;
// 将向量 rvec 转换为 3x3 旋转矩阵 
cv::Rodrigues(rvec, rotation);

cv::solvePnP 函数计算刚性变换(即旋转和平移分量)，将对象坐标带入以相机为中心的参考系(即以焦点为原点的参考系)，此函数计算的旋转是以 3D 矢量的形式给出的。其中要应用的旋转由单位向量(即旋转轴)描述，对象围绕该单位向量旋转某个角度，这种表示方式也称为罗德里格斯旋转公式 (Rodrigues' rotation formula)。在 OpenCV 中，旋转角度对应于输出旋转向量的范数，后者与旋转轴对齐。可以使用 cv::Rodrigues 函数获得出现在投影方程中的 3D 旋转矩阵。

(3) 相机姿势恢复过程十分简单，为了得知我们是否获得了正确的相机和物体姿势信息，可以使用 cv::viz 模块直观地评估结果，cv::viz 模块能够可视化 3D 信息。利用该模块显示对象和相机的简单 3D 表示：

仅通过查看此图像可能难以判断姿势恢复的质量，但可以选择使用鼠标在 3D 中移动观察，以更好地了解所得到的解。
在本节中，我们假设对象的 3D 结构、对象点集和图像点集之间的对应关系是已知的，相机的内在参数也可以通过校准得知(即第一个矩阵的元素)；根据投影方程，这意味着有一些点的坐标是已知的，例如 $(X,Y,Z)$ 和 $(x,y)$。只有第二个矩阵未知，这是一个包含相机的外在参数(即相机/物体姿态信息)的矩阵。因此，我们的目标是从 3D 场景点的观察中恢复这些未知参数。此问题称为 PnP (Perspective-n-Point) 问题。
旋转具有三个自由度(可以围绕三个轴旋转)，平移同样也是如此，因此，共有六个未知数。对于每个物点和像点的对应关系，投影方程给出三个代数方程，但由于射影方程有一个比例因子，因此只有两个独立的方程。因此，求解该方程组至少需要三个点，更多的点可以得到更可靠的估计。
OpenCV 在 cv::solvePnP 函数中实现了多种不同算法。默认方法基于优化重投影误差，最大限度地减少此类误差是从相机图像中获取准确 3D 信息的最佳策略。在 PnP 问题中，对应于找到最佳相机位置，以最小化投影 3D 点(通过应用投影方程获得)和输入观察图像点之间的 2D 距离。
OpenCV 还提供了 cv::solvePnPRansac 函数使用随机抽样一致算法 (RANdom SAmple Consensus, RANSAC) 来解决 PnP 问题。这意味着某些对象点和图像点的对应关系可能是错误的，该函数返回被识别为异常值的对应点，这对于自动获取的对应关系非常有用。

2. 3D 可视化模块 cv::Viz

在处理 3D 信息时，通常很难验证获得的解。为此，OpenCV 提供了一个功能强大的可视化模块，以便于 3D 视觉算法的开发和调试。它允许在虚拟 3D 环境中插入点、线、相机和其他对象，还可以从不同的角度进行交互可视化。
cv::Viz 是 OpenCV 库的一个模块，它构建在可视化工具包 (Visualization Toolkit, VTK) 之上，是一个用于 3D 计算机图形的强大框架。使用 cv::viz，可以创建一个 3D 虚拟环境，可以在其中添加各种对象。创建一个可视化窗口，从给定的角度显示环境。在本节中，学习如何在 cv::viz 窗口中显示内容，此窗口内可以响应鼠标事件(通过旋转和平移)，本节将介绍 cv::viz 模块的基本用法。

(1) 首先要创建可视化窗口，例如，使用白色背景：

// 创建 viz 窗口
cv::viz::Viz3d visualizer("Viz window");
visualizer.setBackgroundColor(cv::viz::Color::white());

(2) 接下来，创建虚拟对象并将它们插入到场景中。OpenCV 中包含多种预定义对象，其中包含用于创建虚拟针孔相机的工具：

// 创建虚拟相机
cv::viz::WCameraPosition cam(cMatrix,   // 内在矩阵 
                        image,          // 图像平面
                        30.0,           // 缩放因子
                        cv::viz::Color::black());
visualizer.showWidget("Camera", cam);

(3) cMatrix 变量是 cv::Matx33d 实例，即 cv::Matx<double,3,3>，其中包含从校准中获得的内在相机参数。默认情况下，将相机插入坐标系的原点，为了表示长凳，我们使用两个矩形长方体对象：

// 使用长方体创建虚拟长凳
cv::viz::WCube plane1(cv::Point3f(0.0, 45.0, 0.0), 
                        cv::Point3f(242.5, 21.0, -9.0), 
                        true,           // 显示线框
                        cv::viz::Color::blue());
plane1.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
cv::viz::WCube plane2(cv::Point3f(0.0, 9.0, -9.0), 
                        cv::Point3f(242.5, 0.0, 44.5), 
                        true,           // 显示线框
                        cv::viz::Color::blue());
plane2.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
// 在环境中添加虚拟对象
visualizer.showWidget("top", plane1);
visualizer.showWidget("bottom", plane2);

(4) 虚拟长凳也是在原点添加的，然后将它移动到以相机为中心(利用 cv::solvePnP 函数)的位置。setWidgetPose 方法可以用于添加操作，该函数应用了估计运动的旋转和平移分量：

cv::Mat rotation;
// 将向量 rvec 转换为 3x3 旋转矩阵 
cv::Rodrigues(rvec, rotation);
// 移动长凳
cv::Affine3d pose(rotation, tvec);
visualizer.setWidgetPose("top", pose);
visualizer.setWidgetPose("bottom", pose);

(5) 最后创建一个不断显示可视化窗口的循环，并在循环中进行 1ms 暂停以侦听鼠标事件：

// 可视化虚拟环境
while(cv::waitKey(100)==-1 && !visualizer.wasStopped())
{
    
    
    visualizer.spinOnce(1,      // 暂停 1ms 
                        true);  // 重绘
}

当可视化窗口关闭或在 OpenCV 图像窗口上按下某个按键时，循环将停止。在此循环中将运动应用于对象就可以创建动画。

3. 完整代码

完整代码 cameraPose.cpp 如下所示：

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>

int main() {
    
    
    // 读取相机标定参数
    cv::Mat cameraMatrix;
    cv::Mat cameraDistCoeffs;
    cv::FileStorage fs("calib.xml", cv::FileStorage::READ);
    fs["Intrinsic"] >> cameraMatrix;
    fs["Distortion"] >> cameraDistCoeffs;
    std::cout << " Camera intrinsic: " << cameraMatrix.rows << "x" << cameraMatrix.cols << std::endl;
    std::cout << cameraMatrix.at<double>(0, 0) << " " << cameraMatrix.at<double>(0, 1) << " " << cameraMatrix.at<double>(0, 2) << std::endl;
    std::cout << cameraMatrix.at<double>(1, 0) << " " << cameraMatrix.at<double>(1, 1) << " " << cameraMatrix.at<double>(1, 2) << std::endl;
    std::cout << cameraMatrix.at<double>(2, 0) << " " << cameraMatrix.at<double>(2, 1) << " " << cameraMatrix.at<double>(2, 2) << std::endl << std::endl;
    cv::Matx33d cMatrix(cameraMatrix);
    // 输入图像点
    std::vector<cv::Point2f> imagePoints;
    imagePoints.push_back(cv::Point2f(207, 60));
    imagePoints.push_back(cv::Point2f(670, 103));
    imagePoints.push_back(cv::Point2f(637, 240));
    imagePoints.push_back(cv::Point2f(183, 156));
    imagePoints.push_back(cv::Point2f(185, 179));
    imagePoints.push_back(cv::Point2f(633, 273));
    imagePoints.push_back(cv::Point2f(541, 332));
    imagePoints.push_back(cv::Point2f(73, 213));
    // 输入对象点
    std::vector<cv::Point3f> objectPoints;
    objectPoints.push_back(cv::Point3f(0, 45, 0));
    objectPoints.push_back(cv::Point3f(242.5, 45, 0));
    objectPoints.push_back(cv::Point3f(242.5, 21, 0));
    objectPoints.push_back(cv::Point3f(0, 21, 0));
    objectPoints.push_back(cv::Point3f(0, 9, -9));
    objectPoints.push_back(cv::Point3f(242.5, 9, -9));
    objectPoints.push_back(cv::Point3f(242.5, 9, 44.5));
    objectPoints.push_back(cv::Point3f(0, 9, 44.5));
    // 读取图像
    cv::Mat image = cv::imread("example_1.jpg");
    // 绘制图像点
    for (int i = 0; i < 8; i++) {
    
    
        cv::circle(image, imagePoints[i], 3, cv::Scalar(0, 0, 0),2);
    }
    cv::imshow("An image of a bench", image);
    // 创建 viz 窗口
    cv::viz::Viz3d visualizer("Viz window");
    visualizer.setBackgroundColor(cv::viz::Color::white());
    // 创建虚拟相机
    cv::viz::WCameraPosition cam(cMatrix,   // 内在矩阵 
                            image,          // 图像平面
                            30.0,           // 缩放因子
                            cv::viz::Color::black());
    // 使用长方体创建虚拟长凳
    cv::viz::WCube plane1(cv::Point3f(0.0, 45.0, 0.0), 
                            cv::Point3f(242.5, 21.0, -9.0), 
                            true,           // 显示线框
                            cv::viz::Color::blue());
    plane1.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
    cv::viz::WCube plane2(cv::Point3f(0.0, 9.0, -9.0), 
                            cv::Point3f(242.5, 0.0, 44.5), 
                            true,           // 显示线框
                            cv::viz::Color::blue());
    plane2.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
    // 在环境中添加虚拟对象
    visualizer.showWidget("top", plane1);
    visualizer.showWidget("bottom", plane2);
    visualizer.showWidget("Camera", cam);
    // 根据 3D/2D 点计算相机姿态
    cv::Mat rvec, tvec;
    cv::solvePnP(objectPoints, imagePoints,         // 对应的 3D/2D 点
                    cameraMatrix, cameraDistCoeffs, // 标定
                    rvec, tvec);                    // 输出姿态
    std::cout << " rvec: " << rvec.rows << "x" << rvec.cols << std::endl;
    std::cout << " tvec: " << tvec.rows << "x" << tvec.cols << std::endl;
    cv::Mat rotation;
    // 将向量 rvec 转换为 3x3 旋转矩阵 
    cv::Rodrigues(rvec, rotation);
    // 移动长凳
    cv::Affine3d pose(rotation, tvec);
    visualizer.setWidgetPose("top", pose);
    visualizer.setWidgetPose("bottom", pose);
    // 可视化虚拟环境
    while(cv::waitKey(100)==-1 && !visualizer.wasStopped())
    {
    
    
        visualizer.spinOnce(1,      // 暂停 1ms 
                            true);  // 重绘
    }
    cv::waitKey();
    return 0;
}

小结

相机位姿估计就是通过几个已知坐标的特征点，以及这些点在照片中的成像位置，求解出相机位于坐标系内的坐标与旋转角度，本节使用 cv::solvePnP 函数估计相机位置，并介绍了如何在 OpenCV 中可视化 3D 场景。

系列链接

OpenCV实战（1）——OpenCV与图像处理基础
OpenCV实战（2）——OpenCV核心数据结构
OpenCV实战（3）——图像感兴趣区域
OpenCV实战（4）——像素操作
OpenCV实战（5）——图像运算详解
OpenCV实战（6）——OpenCV策略设计模式
OpenCV实战（7）——OpenCV色彩空间转换
OpenCV实战（8）——直方图详解
OpenCV实战（9）——基于反向投影直方图检测图像内容
OpenCV实战（10）——积分图像详解
OpenCV实战（11）——形态学变换详解
OpenCV实战（12）——图像滤波详解
OpenCV实战（13）——高通滤波器及其应用
OpenCV实战（14）——图像线条提取
OpenCV实战（15）——轮廓检测详解
OpenCV实战（16）——角点检测详解
OpenCV实战（17）——FAST特征点检测
OpenCV实战（18）——特征匹配
OpenCV实战（19）——特征描述符
OpenCV实战（20）——图像投影关系
OpenCV实战（21）——基于随机样本一致匹配图像
OpenCV实战（22）——单应性及其应用
OpenCV实战（23）——相机标定