基于 OpenCV C++ 实现的纹理对象实时姿态估计系统。文章详细阐述了从 3D 模型注册到实时检测的完整流程，包括 ORB 特征提取、Flann 匹配、PnP+RANSAC 姿态解算以及卡尔曼滤波优化。内容涵盖理论背景、源代码结构解析及关键算法实现细节，旨在为计算机视觉和机器人领域的开发者提供实用的 6-DOF 姿态跟踪方案。
如今，增强现实（AR）是计算机视觉和机器人领域的热门研究课题之一。增强现实中最基本的问题是估计相机相对于物体的姿态。在计算机视觉领域，这通常用于后续的 3D 渲染；在机器人领域，则是为了获取物体姿势以进行抓取和操作。然而，这并不是一个微不足道的问题，因为图像处理中常见的挑战在于应用大量算法或数学运算来解决人类看似简单直接的问题，其计算成本往往较高。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online
在本教程中，将介绍如何构建一个实时应用程序来估计相机姿态，以便在给定 2D 图像及其 3D 纹理模型的情况下跟踪具有六个自由度（6-DOF）的纹理对象。
该应用程序将包含以下核心部分：
在计算机视觉中，从 n 个 3D 点到 2D 点的对应关系来估计相机姿态是一个基本且易于理解的问题。该问题的最一般版本需要估计姿态的六个自由度和五个校准参数：焦距、主点、纵横比和偏斜。它可以使用众所周知的直接线性变换（DLT）算法建立至少 6 个对应关系。
但是，对这个问题进行了一些简化，这些简化变成了一系列不同的算法，这些算法可以提高 DLT 的准确性。最常见的简化是假设已知的校准参数，即所谓的透视-n 点问题（Perspective-n-Point, PnP）。
问题表述： 给定在世界参考系中表示的 3D 点 (p_i) 与它们在图像上的 2D 投影 (u_i) 之间的一组对应关系，我们试图检索相机在世界坐标系中的姿态（旋转矩阵 R 和平移向量 t）以及焦距 f。
OpenCV 提供了四种不同的方法来解决 Perspective-n-Point 问题，它们返回 R 和 t。然后，使用以下公式可以将 3D 点投影到图像平面中：
u = K * [R | t] * P_world
其中 K 是相机内参矩阵。有关如何使用此方程式进行管理的完整文档，请参阅 OpenCV 官方文档。
您可以在 OpenCV 源码库的文件夹中找到本教程的源代码。路径通常为 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/。
本教程由两个主要程序组成：
此应用程序仅供没有要检测的物体的 3D 纹理模型的人使用。您可以使用此程序创建自己的纹理 3D 模型。该程序仅适用于平面对象，如果您想对具有复杂形状的对象进行建模，则应使用复杂的软件来创建它。
应用程序需要要注册的对象及其 3D 网格的输入图像。我们还必须提供用于拍摄输入图像的相机的固有参数。所有文件都需要使用绝对路径或应用程序工作目录中的相对路径来指定。如果未指定任何文件，程序将尝试打开提供的默认参数。
应用程序开始从输入图像中提取 ORB 特征和描述符，然后使用网格和内参来计算找到的特征的 3D 坐标。最后，3D 点和描述符存储在 YAML 格式的文件中的不同列表中，每行都是一个不同的点。
此应用程序的目的是根据其 3D 纹理模型实时估计物体的姿态。
应用程序开始以 YAML 文件格式加载 3D 纹理模型，其结构与模型注册程序中解释的结构相同。从场景中，检测并提取 ORB 特征和描述符。然后，使用 cv::FlannBasedMatcher 在场景描述符和模型描述符之间进行匹配。使用找到的匹配项以及 solvePnPRansac 函数计算相机的 R 和 t。最后，应用卡尔曼滤波器来拒绝不良姿势。
如果您使用示例编译了 OpenCV，则可以在 opencv/build/bin/cpp-tutorial-pnp_detection 中找到它。然后，您可以运行应用程序并更改一些参数。
该程序展示了如何在给定 3D 纹理模型的情况下检测对象。您可以选择使用录制的视频或网络摄像头。
用法：
./cpp-tutorial-pnp_detection -help
参数说明：
-c, --confidence: 置信度（默认值：0.95）-e, --error: 重投影误差（默认值：2.0）-f, --fast: 使用稳健的快速匹配（默认值：true）-h, --help: 显示帮助信息--inliers: 卡尔曼更新的最小内点数（默认值：30）--iterations: 最大迭代计数（默认值：500）-k, --keypoints: 要检测的关键点数（默认值：2000）--mesh: 网格铺层的路径--method: PnP 方法（0: Iterative, 1: EPNP, 2: P3P, 3: DLS）--model: YML 模型的路径-r, --ratio: 比率测试阈值（默认值：0.7）-v, --video: 录制视频的路径例如，您可以运行应用程序更改 pnp 方法：
./cpp-tutorial-pnp_detection --method=2
这里详细解释了实时应用程序的代码逻辑：
为了加载纹理模型，我实现了 Model 类，该类具有函数 load()，该函数打开一个 YAML 文件并获取存储的 3D 点及其相应的描述符。
// 使用 OpenCV 加载 YAML 文件
void Model::load(const std::string& path)
{
Mat points3d_mat;
FileStorage storage(path, FileStorage::READ);
storage["points_3d"] >> points3d_mat;
storage["descriptors"] >> descriptors_;
list_points3d_in_model.clear();
storage.release();
}
在主程序中，模型按如下方式加载：
Model model; // 实例化 Model 对象
model.load(yml_read_path); // 加载 3D 纹理对象模型
为了读取模型网格，我实现了一个 Mesh 类，它有一个函数 load()，它打开一个 *.ply 文件并存储对象的 3D 点以及组合的三角形。
// 加载 *.ply 格式的 CSV
void Mesh::load(const std::string& path)
{
CsvReader csvReader(path);
list_vertex_.clear();
list_triangles_.clear();
csvReader.readPLY(list_vertex_, list_triangles_);
num_vertexs_ = list_vertex_.size();
num_triangles_ = list_triangles_.size();
}
在主程序中，网格的加载方式如下：
Mesh mesh; // 实例化 Mesh 对象
mesh.load(ply_read_path); // 加载对象网格
您也可以加载不同的模型和网格：
./cpp-tutorial-pnp_detection --mesh=/absolute_path_to_your_mesh.ply --model=/absolute_path_to_your_model.yml
检测是必要的，捕获视频。它通过传递录制的视频在计算机中所在的绝对路径来加载录制的视频。
VideoCapture cap; // 实例化 VideoCapture
cap.open(video_read_path); // 打开录制的视频
if (!cap.isOpened()) // 检查我们是否成功
{
std::cout << "无法打开相机设备" << std::endl;
return -1;
}
然后计算算法每帧：
Mat frame, frame_vis;
while (cap.read(frame) && waitKey(30) != 27) // 捕获帧，直到按下 ESC
{
frame_vis = frame.clone(); // 刷新可视化框架
// 主算法
}
下一步是检测场景特征并提取其描述符。对于这个任务，我实现了一个 RobustMatcher 类，它具有关键点检测和特征提取功能。
在 RobustMatcher 对象中，您可以使用 OpenCV 的任何 2D 特征检测器。在这种情况下，我使用了 ORB 功能，因为它基于 cv::FAST 来检测关键点，并使用 cv::ORB::create() 来提取描述符，这意味着它对旋转是快速和稳健的。
以下代码是如何实例化和设置特征检测器和描述符提取器的：
RobustMatcher rmatcher; // 实例化 RobustMatcher
Ptr<FeatureDetector> detector = makePtr<cv::ORB>(numKeyPoints); // 实例化 ORB 特征检测器
Ptr<DescriptorExtractor> extractor = makePtr<cv::ORB>(); // 实例化 ORB 描述符提取器
rmatcher.setFeatureDetector(detector); // 设置特征检测器
rmatcher.setDescriptorExtractor(extractor); // 设置描述符提取器
特征和描述符将由匹配函数中的 RobustMatcher 计算。
这是我们检测算法的第一步。主要思想是将场景描述符与我们的模型描述符相匹配，以便了解当前场景中发现的特征的 3D 坐标。
首先，我们必须设置要使用的匹配器。在这种情况下，使用 cv::FlannBasedMatcher 匹配器，就计算成本而言，它比 brute-force 匹配器更快，因为我们增加了经过训练的特征集合。对于 FlannBased 匹配器，创建的索引是 Multi-Probe LSH，这对于 ORB 描述符的二进制特性非常高效。
您可以调整 LSH 和搜索参数，以提高匹配效率：
Ptr<LshIndexParams> indexParams = makePtr<LshIndexParams>(6, 12, 1); // 实例化 LSH 索引参数
Ptr<SearchParams> searchParams = makePtr<SearchParams>(50); // 实例化 flann 搜索参数
Ptr<DescriptorMatcher> matcher = new FlannBasedMatcher(indexParams, searchParams); // 实例化 FlannBased 匹配器
rmatcher.setDescriptorMatcher(matcher); // 设置匹配器
其次，我们必须使用 robustMatch() 或 fastRobustMatch() 函数调用匹配器。使用这两个函数的区别在于其计算成本。第一种方法速度较慢，但在过滤良好匹配方面更可靠，因为使用两个比率测试和一个对称性测试。相比之下，第二种方法速度更快，但鲁棒性较差，因为仅对匹配项应用单个比率测试。
下面的代码是获取模型 3D 点及其描述符，然后在主程序中调用匹配器：
std::vector<Point3f> list_points3d_model = model.get_points3d(); // 带有模型 3D 坐标的列表
Mat descriptors_model = model.get_descriptors(); // 包含每个 3D 坐标描述符的列表
// 第 1 步：模型描述符和场景描述符之间的鲁棒匹配
std::vector<DMatch> good_matches; // 获取场景中的模型 3D 点
std::vector<KeyPoint> keypoints_scene; // 获取场景的 2D 点
if (fast_match)
{
rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model);
}
else
{
rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model);
}
在匹配过滤之后，我们必须使用获得的 DMatches 向量从找到的场景关键点和我们的 3D 模型中减去 2D 和 3D 对应关系。
// 第 2 步：找出 2D/3D 对应关系
std::vector<Point3f> list_points3d_model_match; // 在场景中找到的模型 3D 坐标的容器
std::vector<Point2f> list_points2d_scene_match; // 在场景中找到的模型 2D 坐标的容器
for (unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
{
Point3f point3d_model = list_points3d_model[good_matches[match_index].trainIdx]; // 模型的 3D 点
Point2f point2d_scene = keypoints_scene[good_matches[match_index].queryIdx].pt; // 场景的 2D 点
list_points3d_model_match.push_back(point3d_model); // 添加 3D 点
list_points2d_scene_match.push_back(point2d_scene); // 添加 2D 点
}
您还可以更改比率测试阈值、要检测的关键点数以及是否使用鲁棒匹配器。
一旦有了 2D 和 3D 对应关系，我们就必须应用 PnP 算法来估计相机姿势。我们必须使用 cv::solvePnPRansac 而不是 solvePnP 的原因是，在匹配之后，并非所有找到的对应关系都是正确的，并且存在错误的对应关系或也称为异常值（Outliers）。RANSAC 是一种非确定性迭代方法，它根据观察到的数据估计数学模型的参数，随着迭代次数的增加产生近似结果。应用 RANSAC 后，将消除所有异常值，然后以一定的概率估计相机姿态以获得良好的解决方案。
对于相机姿态估计，我实现了一个 PnPProblem 类。此类有 4 个属性：给定校准矩阵、旋转矩阵、平移矩阵和旋转平移矩阵。用于估计姿态的相机的固有校准参数是必要的。
下面的代码是如何在主程序中声明 PnPProblem 类：
// 固有相机参数：UVC WEBCAM
double f = 55; // 焦距（mm）
double SX = 22.3, SY = 14.9; // 传感器尺寸
double width = 640, height = 480; // 图像尺寸
double params_WEBCAM[] = { width*f/SX, // fx
height*f/SY, // fy
width/2, // cx
height/2 }; // cy
PnPProblem pnp_detection(params_WEBCAM); // 实例化 PnPProblem 类
OpenCV 提供四种 PnP 方法：迭代、EPNP、P3P 和 DLS。根据应用程序类型，估计方法会有所不同。如果我们想进行实时应用程序，更合适的方法是 EPNP 和 P3P，因为它们在寻找最佳解决方案方面比 ITERATIVE 和 DLS 更快。然而，EPNP 和 P3P 在平面前并不是特别鲁棒，有时姿态估计似乎具有镜像效应。因此，在本教程中，由于要检测的对象具有平面，因此使用了迭代方法。
OpenCV RANSAC 实现希望您提供三个参数：1）算法停止之前的最大迭代次数，2）观测点投影和计算点投影之间允许的最大距离，以将其视为内点（Inlier），以及 3）获得良好结果的置信度。
以下参数适用于此应用程序：
以下代码对应于属于 PnPProblem 类的 estimatePoseRANSAC() 函数。此函数在给定一组 2D/3D 对应关系、要使用的所需 PnP 方法、输出 inliers 容器和 RANSAC 参数的情况下估计旋转和平移矩阵：
void PnPProblem::estimatePoseRANSAC(
const std::vector<Point3f>& list_points3d,
const std::vector<Point2f>& list_points2d,
int flags,
vector<int>& inliers,
int iterationsCount,
float reprojectionError,
float confidence)
{
Mat distCoeffs = Mat::zeros(4, 1, CV_64F); // 畸变系数向量
Mat rvec = Mat::zeros(3, 1, CV_64F); // 输出旋转矢量
Mat tvec = Mat::zeros(3, 1, CV_64F); // 输出翻译向量
bool useExtrinsicGuess = false;
solvePnPRansac(list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
inliers, flags);
Rodrigues(rvec, _R_matrix); // 将 Rotation Vector 转换为 Matrix
_t_matrix = tvec; // 设置翻译矩阵
this->set_P_matrix(_R_matrix, _t_matrix); // 设置旋转平移矩阵
}
以下代码是主算法的第 3 步和第 4 步。第一个调用上述函数，第二个从 RANSAC 获取输出内值向量以获取用于绘图目的的 2D 场景点。如代码所示，如果我们有匹配项，我们必须确保应用 RANSAC，在另一种情况下，函数可能会由于任何 OpenCV 错误而崩溃。
if (good_matches.size() > 0) // 不匹配，则 RANSAC 崩溃
{
// 第 3 步：使用 RANSAC 方法估计姿势
pnp_detection.estimatePoseRANSAC(list_points3d_model_match, list_points2d_scene_match,
pnpMethod, inliers_idx, iterationsCount, reprojectionError, confidence);
// 第 4 步：捕捉要绘制的 inliers 关键点
for (int inliers_index = 0; inliers_index < inliers_idx.rows; ++inliers_index)
{
int n = inliers_idx.at(inliers_index); // i-inlier
Point2f point2d = list_points2d_scene_match[n]; // i-inlier 点 2D
list_points2d_inliers.push_back(point2d); // 将 i-inlier 添加到列表中
}
}
最后，一旦估计了相机姿态，我们就可以使用 R 和 t 来计算在世界参考系中表示的给定 3D 点的图像上的 2D 投影。
Point2f PnPProblem::backproject3DPoint(const Point3f& point3d)
{
// 3D 点向量 [x y z 1]'
Mat point3d_vec = (Mat_<double>(4, 1) << point3d.x, point3d.y, point3d.z, 1.0);
// 二维点向量 [u v 1]'
Mat point2d_vec = _A_matrix * _P_matrix * point3d_vec;
// [u v]' 的归一化
Point2f point2d;
point2d.x = point2d_vec.at<double>(0) / point2d_vec.at<double>(2);
point2d.y = point2d_vec.at<double>(1) / point2d_vec.at<double>(2);
return point2d;
}
上面的函数用于计算对象网格的所有 3D 点，以显示对象的姿态。
在计算机视觉或机器人领域，在应用检测或跟踪技术后，由于某些传感器错误而获得不良结果是否常见。为了避免这些不良检测，本教程将介绍如何实现线性卡尔曼滤波。卡尔曼滤波将在检测到给定数量的异常值后应用。
在本教程中，它使用基于 KalmanFilter 的 OpenCV 实现来设置动力学和测量模型，以进行位置和方向跟踪。
首先，我们必须定义我们的状态向量，它将有 18 个状态：位置数据 (x, y, z) 及其一阶和二阶导数（速度和加速度），然后以三个欧拉角（滚动、俯仰、偏航）的形式将旋转与它们的一阶和二阶导数（角速度和加速度）相加。
这种设计允许滤波器平滑噪声并预测下一帧的姿态，从而在特征匹配暂时失败时保持稳定的跟踪效果。
以下视频是使用以下参数使用说明的检测算法实时进行姿态估计的结果：
鲁棒匹配器参数
RANSAC 参数
卡尔曼滤波参数
通过调整上述参数，可以平衡检测速度与精度，以适应不同的硬件环境和应用场景。在实际部署中，建议根据具体的相机帧率和物体运动速度进行微调。