相机标定与3D重建——纹理对象的实时姿态估计 OpenCV v4.8.0

上一个教程 ： 使用 OpenCV 标定相机

下一个教程 ： 交互式相机标定应用程序

原作者	Edgar Riba
兼容性	OpenCV >= 3.0

如今，增强现实技术已成为计算机视觉和机器人领域的热门研究课题之一。在计算机视觉领域，增强现实最基本的问题是估计物体的相机姿态，以便随后进行三维渲染；在机器人领域，则是获取物体姿态，以便抓住它并进行一些操作。然而，要解决这个问题并非易事，因为图像处理中最常见的问题是，要解决一个对人类来说最基本、最直接的问题，需要应用大量算法或数学运算，计算成本很高。

目标

本教程将介绍如何创建一个实时应用程序来估计摄像机的姿态，以便在给定二维图像及其三维纹理模型的情况下，跟踪一个具有六个自由度的纹理物体。

应用程序将包括以下部分：

• 读取三维纹理对象模型和对象网格。
• 接收来自摄像头或视频的输入。
• 从场景中提取 ORB 特征和描述符。
• 使用 Flann 匹配器匹配场景描述符和模型描述符。
• 使用 PnP + Ransac 进行姿态估计。
• 线性卡尔曼滤波器用于剔除不良姿势。

理论

在计算机视觉领域，从 n 个三维到二维的点对应关系中估算摄像机姿态是一个基本且广为人知的问题。该问题的最一般版本需要估计姿势的六个自由度和五个校准参数：焦距、主点、长宽比和倾斜度。使用众所周知的直接线性变换（DLT）算法，至少可以建立 6 个对应关系。不过，这个问题也有几种简化方法，这些简化方法可以提高直接线性变换算法的准确性。

最常见的简化是假设已知的校准参数，这就是所谓的透视n点问题：

问题表述： 给定一组以世界参考框架表示的三维点 pi 与它们在图像上的二维投影 ui 之间的对应关系，我们试图检索摄像机相对于世界和焦距 f 的姿态 ( R 和 t)。

OpenCV 提供了四种不同的方法来解决透视-n-点（Perspective-n-Point）问题，并返回 R 和 t：

$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$
有关如何管理此方程的完整文档，请参阅 Camera Calibration and 3D Reconstruction 。

源代码

您可以在 OpenCV 源代码库的 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/ 文件夹中找到本教程的源代码。

本教程由两个主要程序组成：

1. 模型标记

该程序只适用于没有三维纹理模型的检测对象。您可以使用该程序创建自己的三维纹理模型。该程序只适用于平面物体，如果您想为形状复杂的物体建模，则应使用复杂的软件来创建。

该程序需要输入要注册对象的图像及其 3D 网格。我们还必须提供拍摄输入图像的相机的固有参数。所有文件都需要使用应用程序工作目录中的绝对路径或相对路径指定。如果未指定任何文件，程序将尝试打开所提供的默认参数。

程序启动后会从输入图像中提取 ORB 特征和描述符，然后使用网格和 Möller-Trumbore 相交算法计算所发现特征的三维坐标。最后，三维点和描述符被存储在 YAML 格式文件的不同列表中，每一行都是一个不同的点。关于如何存储文件的技术背景，请参阅 使用 XML 和 YAML 文件进行文件输入和输出 教程。

2. 模型检测

该程序的目的是根据三维纹理模型实时估算物体的姿态。

程序启动时会加载 YAML 文件格式的三维纹理模型，其结构与模型注册程序中解释的相同。从场景中检测并提取 ORB 特征和描述符。然后，使用 cv::FlannBasedMatcher 和 cv::flann::GenericIndex，在场景描述符和模型描述符之间进行匹配。使用找到的匹配结果和 cv::solvePnPRansac 函数计算摄像机的 R 和 t。最后，应用卡尔曼滤波器（KalmanFilter）剔除不良姿势。

如果您使用样本编译了 OpenCV，可以在 opencv/build/bin/cpp-tutorial-pnp_detection` 中找到它。然后运行程序并更改一些参数：

This program shows how to detect an object given its 3D textured model. You can choose to use a recorded video or the webcam.
Usage:
 ./cpp-tutorial-pnp_detection -help
Keys:
 'esc' - to quit.
--------------------------------------------------------------------------
Usage: cpp-tutorial-pnp_detection [params]
 -c, --confidence (value:0.95)
 RANSAC confidence
 -e, --error (value:2.0)
 RANSAC reprojection error
 -f, --fast (value:true)
 use of robust fast match
 -h, --help (value:true)
 print this message
 --in, --inliers (value:30)
 minimum inliers for Kalman update
 --it, --iterations (value:500)
 RANSAC maximum iterations count
 -k, --keypoints (value:2000)
 number of keypoints to detect
 --mesh
 path to ply mesh
 --method, --pnp (value:0)
 PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS
 --model
 path to yml model
 -r, --ratio (value:0.7)
 threshold for ratio test
 -v, --video
 path to recorded video

例如，您可以更改 pnp 方法运行应用程序：

./cpp-tutorial-pnp_detection --method=2

说明

下面将详细解释实时应用程序的代码：

1. 读取三维纹理对象模型和对象网格。

为了加载纹理模型，我实现了 Model 类，该类具有 load() 函数，可以打开 YAML 文件并读取存储的 3D 点及其相应的描述符。您可以在 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml 中找到三维纹理模型的示例。

/* Load a YAML file using OpenCV */
void Model::load(const std::string path)
{
 cv::Mat points3d_mat;
 cv::FileStorage storage(path, cv::FileStorage::READ);
 storage["points_3d"] >> points3d_mat;
 storage["descriptors"] >> descriptors_;
 points3d_mat.copyTo(list_points3d_in_);
 storage.release();
}

在主程序中，模型的加载过程如下：

Model model; // 实例化模型对象
model.load(yml_read_path); // 加载三维纹理对象模型

为了读取模型网格，我实现了一个 Mesh 类，该类有一个 load() 函数，可以打开一个 ∗.ply 文件，并存储对象的三维点和组成的三角形。您可以在 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply 中找到模型网格的示例。

/* 加载*.ply 格式的 CSV */
void Mesh::load(const std::string path)
{
 // 创建阅读器
 CsvReader csvReader(path)；
 // 清除之前的数据
 list_vertex_.clear()；
 list_triangles_.clear()；
 // 从 .ply 文件读取数据
 csvReader.readPLY(list_vertex_, list_triangles_)；
 // 更新网格属性
 num_vertexs_ = list_vertex_.size()；
 num_triangles_ = list_triangles_.size()；
}

在主程序中，网格的加载过程如下：

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

2. 从摄像头或视频中获取输入

要进行检测，必须捕捉视频。通过传递视频在机器中的绝对路径，可以加载录制的视频。为了测试应用程序，您可以在 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.mp4 中找到录制的视频。

cv::VideoCapture cap; // 实例化 VideoCapture
cap.open(video_read_path); // 打开录制的视频
if(!cap.isOpened()) // 检查是否成功
{
 std::cout << "无法打开摄像机设备" << std::endl；
 return -1；
}

然后逐帧计算算法：

cv::Mat frame, frame_vis；
while(cap.read(frame) && cv::waitKey(30) != 27) // 捕获帧，直到按下 ESC 键
{
 frame_vis = frame.clone(); // 刷新可视化帧
 // 主算法
}

您还可以加载不同的录制视频：

./cpp-tutorial-pnp_detection --video=/absolute_path_to_your_video.mp4

3. 从场景中提取 ORB 特征和描述符

下一步是检测场景特征并提取其描述符。为此，我创建了一个 RobustMatcher 类，该类具有关键点检测和特征提取功能。您可以在 samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobusMatcher.cpp 中找到它。在 RobusMatch 对象中，您可以使用 OpenCV 的任何二维特征检测器。在本例中，我使用了 cv::ORB 特征，因为它基于 cv::FAST 来检测关键点，并使用 cv::xfeatures2d::BriefDescriptorExtractor 来提取描述符，这意味着它不仅速度快，而且不受旋转影响。有关 ORB 的更多详细信息，请参阅文档。

以下代码介绍了如何实例化并设置特征检测器和描述符提取器：

RobustMatcher rmatcher; // 实例化 RobustMatcher
cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints); // 实例化 ORB 特征检测器
cv::DescriptorExtractor * extractor = new cv::OrbDescriptorExtractor(); // 实例化 ORB 描述符提取器
rmatcher.setFeatureDetector(detector); // 设置特征检测器
rmatcher.setDescriptorExtractor(extractor); // 设置描述符提取器

特征和描述符将由匹配函数内的 RobustMatcher 计算。

4. 使用 Flann 匹配器匹配场景描述符和模型描述符

这是我们检测算法的第一步。其主要思路是将场景描述符与我们的模型描述符相匹配，从而知道所发现的特征在当前场景中的三维坐标。

首先，我们必须设置要使用的匹配器。本例中使用的是 cv::FlannBasedMatcher，随着训练的特征集合的增加，它的计算成本比 cv::BFMatcher 更快。由于 ORB 描述符是二进制的，因此基于 FlannBased 的匹配器创建的索引是 Multi-Probe LSH: Efficient Indexing for High-Dimensional Similarity Search。

你可以调整 LSH 和搜索参数来提高匹配效率：

cv::Ptr<cv::flann::IndexParams> indexParams = cv::makePtr<cv::flann::LshIndexParams>(6, 12, 1); // 实例化 LSH 索引参数
cv::Ptr<cv::flann::SearchParams> searchParams = cv::makePtr<cv::flann::SearchParams>(50); // 实例化法兰搜索参数
cv::DescriptorMatcher * matcher = new cv::FlannBasedMatcher(indexParams, searchParams); // 实例化 FlannBased 匹配器
rmatcher.setDescriptorMatcher(matcher); // 设置匹配器

其次，我们必须使用 robustMatch() 或 fastRobustMatch() 函数调用匹配器。使用这两个函数的区别在于其计算成本。第一种方法速度较慢，但由于使用了两个比率测试和一个对称性测试，因此在过滤良好匹配时更加稳健。相比之下，第二种方法速度更快，但鲁棒性较差，因为只对匹配结果进行了一次比率测试。

以下代码用于获取模型三维点及其描述符，然后在主程序中调用匹配器：

// 获取模型信息
std::vector<cv::Point3f> list_points3d_model = model.get_points3d(); // 包含模型三维坐标的列表
cv::Mat descriptors_model = model.get_descriptors(); // 每个三维坐标的描述符列表

// -- 第 1 步：模型描述符与场景描述符之间的稳健匹配
std::vector<cv::DMatch> good_matches; // 获取场景中的模型三维点
std::vector<cv::KeyPoint> keypoints_scene; // 获取场景中的二维点
if(fast_match)
{
 rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model)；
}
else
{
 rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model)；
}

以下代码对应的是属于 RobustMatcher 类的 robustMatch() 函数。该函数使用给定的图像检测关键点并提取描述符，使用两个最近邻法将提取的描述符与给定的模型描述符进行匹配，反之亦然。然后，对两个方向的匹配结果进行比率测试，以剔除第一个和第二个最佳匹配结果之间的距离比大于给定阈值的匹配结果。最后，应用对称性测试来移除不对称的匹配。

void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches、
 std::vector<cv::KeyPoint>& keypoints_frame、
 const std::vector<cv::KeyPoint>& keypoints_model, const cv::Mat& descriptors_model )
{
 // 1a. 检测 ORB 特征
 this->computeKeyPoints(frame, keypoints_frame)；
 // 1b. 提取 ORB 描述符
 cv::Mat descriptors_frame；
 this->computeDescriptors(frame, keypoints_frame, descriptors_frame)；
 // 2.匹配两个图像描述符
 std::vector<std::vector<cv::DMatch> > matches12, matches21；

 // 2a. 从图像 1 到图像 2
 matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours
 // 2b. 从图像 2 到图像 1
 matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // 返回 2 个最近邻域。
 // 删除近邻比大于阈值的匹配项
 // 清除图像 1 -> 图像 2 匹配项
 int removed1 = ratioTest(matches12)；
 // 清理图像 2 -> 匹配图像 1
 int removed2 = ratioTest(matches21)；
 // 4.移除非对称匹配项
 symmetryTest(matches12, matches21, good_matched)；
}

完成匹配过滤后，我们必须使用获得的 DMatches 向量，从找到的场景关键点和三维模型中减去二维和三维对应关系。有关 cv::DMatch 的更多信息，请查看文档。

// -- 第 2 步：找出二维/三维对应关系
std::vector<cv::Point3f> list_points3d_model_match; // 在场景中找到的模型三维坐标的容器
std::vector<cv::Point2f> list_points2d_scene_match; // 在场景中找到的模型二维坐标的容器
for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
{
 cv::Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ]; // 模型中的 3D 点
 cv::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); // 添加二维点
}

您还可以更改比率测试阈值、要检测的关键点数量以及是否使用鲁棒匹配器：

./cpp-tutorial-pnp_detection --ratio=0.8 --keypoints=1000 --fast=false

5. 使用 PnP + Ransac 估算姿势

有了二维和三维对应关系后，我们必须应用 PnP 算法来估计摄像机的姿态。我们之所以要使用 cv::solvePnPRansac 而不是 cv::solvePnP，是因为在匹配之后，并非所有找到的对应关系都是正确的，也有可能存在错误的对应关系或称为离群值。随机抽样共识法或 Ransac 是一种非确定性迭代法，它可以根据观测数据估计数学模型的参数，随着迭代次数的增加而产生近似结果。应用 Ransac 方法后，所有异常值都将被消除，然后以一定的概率估算摄像机姿态，从而获得良好的解决方案。

为了估算摄像机姿态，我创建了一个类 PnPProblem。该类有 4 个属性：给定的校准矩阵、旋转矩阵、平移矩阵和旋转-平移矩阵。用于估计姿态的摄像机固有校准参数是必要的。要获取这些参数，可以查看 方形棋盘的摄像机标定 和 OpenCV 的摄像机标定 教程。

以下代码说明了如何在主程序中声明 PnPProblem 类：

// 相机固有参数： UVC WEBCAM
double f = 55; // 焦距（毫米
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 类

下面的代码介绍了 PnPProblem 类如何初始化其属性：

// 给定摄像机固有参数的自定义构造函数
PnPProblem::PnPProblem(const double params[])
{
 _A_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // 固有摄像机参数
 _A_matrix.at<double>(0, 0) = params[0]; // [ fx 0 cx ].
 _A_matrix.at<double>(1, 1) = params[1]; // [ 0 fy cy ]
 _A_matrix.at<double>(0, 2) = params[2]; // [ 0 0 1 ]
 _A_matrix.at<double>(1, 2) = params[3]；
 _A_matrix.at<double>(2, 2) = 1；
 _R_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // 旋转矩阵
 _t_matrix = cv::Mat::zeros(3, 1, CV_64FC1); // 平移矩阵
 _P_matrix = cv::Mat::zeros(3, 4, CV_64FC1); // 旋转-平移矩阵
}

OpenCV 提供了四种 PnP 方法： ITERATIVE、EPNP、P3P 和 DLS。根据应用类型的不同，估算方法也会不同。如果我们想制作一个实时应用程序，更适合的方法是 EPNP 和 P3P，因为它们比 ITERATIVE 和 DLS 更快找到最优解。不过，EPNP 和 P3P 在平面面前并不特别稳健，有时姿态估计似乎会产生镜像效应。因此，在本教程中，由于要检测的物体具有平面，因此采用了 ITERATIVE 方法。

OpenCV RANSAC 实现需要您提供三个参数： 1) 算法停止前的最大迭代次数；2) 观测点投影与计算点投影之间的最大允许距离，以便将其视为离群点；3) 获得良好结果的置信度。您可以调整这些参数，以提高算法性能。增加迭代次数会得到更精确的解，但需要更多时间来找到解。增加重投误差会减少计算时间，但求解会不准确。降低置信度，算法会更快，但得到的解也会不准确。

以下参数适用于本应用：

// RANSAC 参数
int iterationsCount = 500; // Ransac 的迭代次数。
float reprojectionError = 2.0; // 将其视为离群值的最大允许距离。
float confidence = 0.95; // RANSAC 成功的置信度。

下面的代码对应于属于 PnPProblem 类的 estimatePoseRANSAC() 函数。该函数在给出一组 2D/3D 对应关系、所需使用的 PnP 方法、输出离群值容器和 Ransac 参数的情况下，估算旋转和平移矩阵：

// 利用 RANSAC 和要使用的方法估计姿势，给定 2D/3D 对应关系列表
void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, // 包含模型 3D 坐标的列表
 const std::vector<cv::Point2f> &list_points2d, // 包含场景 2D 坐标的列表
 int flags, cv::Mat &inliers, int iterationsCount, // PnP 方法；inliers 容器
 float reprojectionError, float confidence ) // RANSAC 参数
{
 cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1); // 失真系数向量
 cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // 输出旋转向量
 cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // 输出平移向量
 bool useExtrinsicGuess = false; // 如果为真，函数使用提供的 rvec 和 tvec 值作为
 // 旋转和平移向量的初始近似值
 cv::solvePnPRansac( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec、
 useExtrinsicGuess, iterationsCount, reprojectionError, confidence、
 inliers, flags )；
 Rodrigues(rvec,_R_matrix); // 将旋转向量转换为矩阵
 _t_matrix = tvec; // 设置平移矩阵
 this->set_P_matrix(_R_matrix, _t_matrix); // 设置旋转-平移矩阵
}

下面的代码是主算法的第 3 和第 4 步。第一步是调用上述函数，第二步是获取 RANSAC 输出的离群值向量，从而得到用于绘图的二维场景点。如代码所示，我们必须确保在有匹配结果的情况下应用 RANSAC，否则函数 cv::solvePnPRansac 会因 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 步：捕捉要绘制的离群值关键点
 for(int inliers_index = 0; inliers_index < inliers_idx.rows; ++inliers_index)
 {
 int n = inliers_idx.at<int>(inliers_index)；//第 i 个离群值
 cv::Point2f point2d = list_points2d_scene_match[n]; // i-inlier point 2D
 list_points2d_inliers.push_back(point2d); // 将第 i 个离群点添加到列表中
}

最后，在估算出摄像机姿态后，我们就可以使用 R 和 t，通过理论上的显示公式，计算以世界参照系表示的给定三维点在图像上的二维投影。

下面的代码对应于 PnPProblem 类中的 backproject3DPoint() 函数。该函数将在世界参照系中表达的给定三维点反向投影到二维图像上：

// 使用估计的姿态参数将三维点反向投影到二维图像上
cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
{
 // 三维点向量 [x y z 1]'
 cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1)；
 point3d_vec.at<double>(0) = point3d.x；
 point3d_vec.at<double>(1) = point3d.y；
 point3d_vec.at<double>(2) = point3d.z；
 point3d_vec.at<double>(3) = 1；
 // 二维点矢量 [u v 1]'
 cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1)；
 point2d_vec = _A_matrix * _P_matrix * point3d_vec；
 // 归一化 [u v]'
 cv::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；
}

上述函数用于计算物体网格的所有三维点，以显示物体的姿态。

您还可以更改 RANSAC 参数和 PnP 方法：

./cpp-tutorial-pnp_detection --error=0.25 --confidence=0.90 --iterations=250 --method=3

6. 线性卡尔曼滤波器用于不良姿态剔除

在计算机视觉或机器人领域，应用检测或跟踪技术后，由于某些传感器误差而产生不良结果的情况很常见。为了避免这些糟糕的检测结果，本教程将讲解如何实现线性卡尔曼滤波器。卡尔曼滤波器将在检测到一定数量的异常值后应用。

您可以找到有关卡尔曼滤波器的更多信息。本教程使用基于线性卡尔曼滤波器的 cv::KalmanFilter 的 OpenCV 实现，用于位置和方向跟踪，以设置动态和测量模型。

首先，我们必须定义我们的状态向量，它将有 18 个状态：位置数据（x,y,z）及其一阶导数和二阶导数（速度和加速度），然后以三个欧拉角（滚动角、俯仰角、下颌角）及其一阶导数和二阶导数（角速度和加速度）的形式添加旋转状态

$$X=(x,y,z,\dot{x},\dot{y},\dot{z},\ddot{x},\ddot{y},\ddot{z},\psi ,\theta ,\varphi ,\dot{\psi },\dot{\theta },\dot{\varphi },\ddot{\psi },\ddot{\theta },\ddot{\varphi }{)}^T$$
其次，我们必须定义测量次数，即 6 次：我们可以从 R 和 t 中提取$$(x,y,z)和(\psi ,\theta ,\varphi )$$此外，我们还必须定义应用于系统的控制动作次数，在本例中，控制动作次数为零。最后，我们必须定义测量之间的时间差，在本例中为 1/T，其中 T 为视频帧频。

cv::KalmanFilter KF; // 卡尔曼滤波器实例化
int nStates = 18; // 状态数
int nMeasurements = 6; // 测量状态数
int nInputs = 0; // 动作控制数
double dt = 0.125; // 测量间隔时间（1/FPS）
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // 初始函数

以下代码对应卡尔曼滤波器的初始化。首先，设置过程噪声、测量噪声和误差协方差矩阵。其次，设置过渡矩阵，即动态模型，最后设置测量矩阵，即测量模型。

您可以调整过程噪声和测量噪声，以提高卡尔曼滤波器的性能。随着测量噪声的减小，算法的收敛速度会更快，在不良测量面前也会更敏感。

void initKalmanFilter(cv::KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt)
{
 KF.init(nStates, nMeasurements, nInputs, CV_64F); // 启动卡尔曼滤波器
 cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5)); // 设置过程噪声
 cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-4)); // 设置测量噪声
 cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1)); // 误差协方差
 /* 动态模型
 // [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0 0] // [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0
 // [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0] // [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0
 
 // [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0] // [0 0 0 1 0 0 0 dt 0 0 0 0 0 0
 // [0 0 0 0 1 0 0 0 dt 0 0 0 0 0 0 0 0 0] // [0 0 0 0 1 0 0 0 dt 0 0 0 0 0 0 0
 // [0 0 0 0 0 1 0 0 0 dt 0 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 1 0 0 0 dt 0 0 0 0 0 0 0 0 0
 // [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0]
 // [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]
 // [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0]
 // [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0] // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0
 // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0
 // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2] // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2
 // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0] // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0
 // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0
 
 // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
 // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
 
 // 位置
 KF.transitionMatrix.at<double>(0,3) = dt；
 KF.transitionMatrix.at<double>(1,4) = dt；
 KF.transitionMatrix.at<double>(2,5) = dt；
 KF.transitionMatrix.at<double>(3,6) = dt；
 KF.transitionMatrix.at<double>(4,7) = dt；
 KF.transitionMatrix.at<double>(5,8) = dt；
 KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2)；
 KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2)；
 KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2)；
 // 方向
 KF.transitionMatrix.at<double>(9,12) = 0.5*pow(dt,2); // 方向
 KF.transitionMatrix.at<double>(10,13) = dt；
 KF.transitionMatrix.at<double>(11,14) = dt；
 KF.transitionMatrix.at<double>(12,15) = dt；
 KF.transitionMatrix.at<double>(13,16) = dt；
 KF.transitionMatrix.at<double>(14,17) = dt；
 KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2)；
 KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2)；
 KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2)；
 /* 测量模型
 // [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 // [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 // [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
 
 // [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
 // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
 KF.measurementMatrix.at<double>(0,0) = 1; // x
 KF.measurementMatrix.at<double>(1,1) = 1; // y
 KF.measurementMatrix.at<double>(2,2) = 1; // z
 KF.measurementMatrix.at<double>(3,9) = 1; // 滚转
 KF.measurementMatrix.at<double>(4,10) = 1; // 俯仰
 KF.measurementMatrix.at<double>(5,11) = 1; // 偏航
}

下面的代码是主算法的第 5 步。当 Ransac 计算后得到的离群值数量超过阈值时，测量矩阵将被填满，然后更新卡尔曼滤波器：

// -- 第 5 步：卡尔曼滤波
// 良好测量
if( inliers_idx.rows >= minInliersKalman )
{
 // 获取测量的平移值
 cv::Mat translation_measured(3, 1, CV_64F)；
 translation_measured = pnp_detection.get_t_matrix()；
 // 获取测量到的旋转
 cv::Mat rotation_measured(3, 3, CV_64F)；
 rotation_measured = pnp_detection.get_R_matrix()；
 // 填充测量向量
 fillMeasurements(measurements, translation_measured, rotation_measured)；
}
// 实例化估计的平移和旋转
cv::Mat translation_estimated(3, 1, CV_64F)；
cv::Mat rotation_estimated(3, 3, CV_64F)；
// 使用良好的测量结果更新卡尔曼滤波器
updateKalmanFilter( KF, measurements、
 translation_estimated，rotation_estimated）；

下面的代码对应于 fillMeasurements() 函数，该函数将测量到的旋转矩阵转换为 Eulers 角度，并将测量到的平移矢量一起填充到测量矩阵中：

void fillMeasurements( cv::Mat &measurements、
 const cv::Mat &translation_measured, const cv::Mat &rotation_measured)
{
 // 将旋转矩阵转换为欧拉角
 cv::Mat measured_eulers(3, 1, CV_64F)；
 measured_eulers = rot2euler(rotation_measured)；
 // 设置测量值以预测
 measurements.at<double>(0) = translation_measured.at<double>(0); // x
 measurements.at<double>(1) = translation_measured.at<double>(1); // y
 
 measurements.at<double>(2) = translation_measured.at<double>(2); // z measurements.at<double>(3) = measured_eulers.at<double>(0); // roll
 measurements.at<double>(4) = measured_eulers.at<double>(1); // pitch
 measurements.at<double>(5) = measured_eulers.at<double>(2); // yaw
}

下面的代码对应于 updateKalmanFilter() 函数，该函数用于更新卡尔曼滤波器，并设置估计的旋转矩阵和平移矢量。估计的旋转矩阵来自欧拉角到旋转矩阵的估计值。

void updateKalmanFilter( cv::KalmanFilter &KF, cv::Mat &measurement、
 cv::Mat &translation_estimated, cv::Mat &rotation_estimated )
{
 // 首先预测，更新内部 statePre 变量
 cv::Mat prediction = KF.predict()；
 
 // "正确 "阶段将使用预测值和我们的测量值 cv::Mat estimated = KF.correct(measurement)；
 // 估计翻译
 
 translation_estimated.at<double>(0) = estimated.at<double>(0)；  translation_estimated.at<double>(1) = estimated.at<double>(1)；
 translation_estimated.at<double>(2) = estimated.at<double>(2)；
 // 估计的欧拉角
 cv::Mat eulers_estimated(3, 1, CV_64F)；
 eulers_estimated.at<double>(0) = estimated.at<double>(9)；
 eulers_estimated.at<double>(1) = estimated.at<double>(10)；
 eulers_estimated.at<double>(2) = estimated.at<double>(11)；
 // 将估计的四元数转换为旋转矩阵
 rotation_estimated = euler2rot(eulers_estimated)；
}

第六步是设置估计的旋转-平移矩阵：

// -- 第 6 步：设置估计投影矩阵
pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated)；

最后一步也是可选的一步是绘制找到的姿势。为此，我实现了一个绘制所有网格三维点和额外参考轴的函数：

// -- 第 X 步： 绘制姿势
drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // 绘制当前姿势
drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // 绘制估计姿势
double l = 5；
std::vector<cv::Point2f> pose_points2d；
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,0,0)));//轴中心点
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(l,0,0)); // 轴 x
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,l,0))); // y 轴
 
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,0,l)); // z 轴 draw3DCoordinateAxes(frame_vis, pose_points2d); // 绘制轴线

还可以修改最小离群值来更新卡尔曼滤波器：

./cpp-tutorial-pnp_detection --inliers=20

结果

以下视频是使用所解释的检测算法并使用以下参数进行姿态实时估计的结果：

// 鲁棒匹配器参数
int numKeyPoints = 2000; // 检测到的关键点数量
float ratio = 0.70f; // 比率测试
bool fast_match = true; // fastRobustMatch() 或 robustMatch()
// RANSAC 参数
int iterationsCount = 500; // Ransac 的迭代次数。
int reprojectionError = 2.0; // 将其视为离群值的最大允许距离。
float confidence = 0.95; // RANSAC 成功置信度。
// 卡尔曼滤波参数
int minInliersKalman = 30; // 卡尔曼阈值更新

您可以在 YouTube 上观看实时姿势估计。