纹理对象的实时姿态估计

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

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

详细实现解释

这里详细解释了实时应用程序的代码逻辑：

1. 读取 3D 纹理对象模型和对象网格

为了加载纹理模型，我实现了 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

2. 从相机或视频获取输入

检测是必要的，捕获视频。它通过传递录制的视频在计算机中所在的绝对路径来加载录制的视频。

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(); // 刷新可视化框架
    // 主算法
}

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

下一步是检测场景特征并提取其描述符。对于这个任务，我实现了一个 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 计算。

4. 使用 Flann 匹配器将场景描述符与模型描述符进行匹配

这是我们检测算法的第一步。主要思想是将场景描述符与我们的模型描述符相匹配，以便了解当前场景中发现的特征的 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 点
}

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

5. 使用 PnP + RANSAC 进行姿态估计

一旦有了 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）获得良好结果的置信度。

以下参数适用于此应用程序：

• 迭代计数 = 500 (RANSAC 迭代次数)
• 重投影误差 = 2.0 (最大允许距离，将其视为 Inlier)
• 置信度 = 0.95 (RANSAC 成功置信度)

以下代码对应于属于 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 点，以显示对象的姿态。

6. 线性卡尔曼滤波器，用于不良姿态抑制

在计算机视觉或机器人领域，在应用检测或跟踪技术后，由于某些传感器错误而获得不良结果是否常见。为了避免这些不良检测，本教程将介绍如何实现线性卡尔曼滤波。卡尔曼滤波将在检测到给定数量的异常值后应用。

在本教程中，它使用基于 KalmanFilter 的 OpenCV 实现来设置动力学和测量模型，以进行位置和方向跟踪。

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

这种设计允许滤波器平滑噪声并预测下一帧的姿态，从而在特征匹配暂时失败时保持稳定的跟踪效果。

实验结果

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

鲁棒匹配器参数

• 检测到的关键点数：2000
• 比率测试：0.70
• 快速匹配模式：True

RANSAC 参数

• 迭代次数：500
• 最大重投影误差：2.0
• 成功置信度：0.95

卡尔曼滤波参数

• 最小内点阈值：30

通过调整上述参数，可以平衡检测速度与精度，以适应不同的硬件环境和应用场景。在实际部署中，建议根据具体的相机帧率和物体运动速度进行微调。