【opencv】实时位姿估计（real_time_pose

相机成像原理图

物体网格、关键点（局内点、局外点）图像

box.ply

resized_IMG_3875.JPG

主程序main_registration.cpp

主要实现了利用OpenCV库进行3D模型的注册。主要步骤包括加载3D网格模型、使用鼠标事件选择对应的3D点进行2D到3D的注册、利用solvePnP算法计算摄像机位姿、并将结果保存在yaml文件中。这通常用于计算机视觉中的对象识别和姿态估计。程序也包括了绘制相应点、调试文本和3D物体网格的功能，以便更好地视觉化注册过程。

// 包含C++的输入输出流库
#include <iostream>
// 包含OpenCV的核心、图像处理、相机标定和特征点检测等功能的库
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d.hpp>
// 包含本教程的网格、模型、PNP问题、稳健匹配和模型注册以及工具等类
#include "Mesh.h"
#include "Model.h"
#include "PnPProblem.h"
#include "RobustMatcher.h"
#include "ModelRegistration.h"
#include "Utils.h"


// OpenCV和标准模板库的命名空间
using namespace cv;
using namespace std;


/**  全局变量  **/


// 注册是否完成的布尔型变量
bool end_registration = false;


// 相机的内部参数: UVC WEBCAM
const double f = 45; // 焦距，以毫米为单位
const double sx = 22.3, sy = 14.9; // 传感器尺寸，以毫米为单位
const double width = 2592, height = 1944; // 图像宽度和高度
const double params_CANON[] = { width*f/sx,   // fx
                                height*f/sy,  // fy
                                width/2,      // cx
                                height/2};    // cy


// 设置在图像中要注册的点
// 根据*.ply文件的顺序，从1开始
const int n = 8;
const int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 3 -> 4


/*
 * 创建模型注册对象
 * 创建网格对象
 * 创建模型对象
 * 创建PNP问题对象
 */
ModelRegistration registration;
Model model;
Mesh mesh;
PnPProblem pnp_registration(params_CANON);


/**********************************************************************************************************/
// 显示帮助信息的函数
static void help()
{
    cout
            << "--------------------------------------------------------------------------"   << endl
            << "这个程序展示了如何创建你的3D贴图模型。"                                      << endl
            << "使用方法："                                                                    << endl
            << "./cpp-tutorial-pnp_registration"                                              << endl
            << "--------------------------------------------------------------------------"   << endl
            << endl;
}


// 鼠标事件回调函数，用于模型注册
static void onMouseModelRegistration( int event, int x, int y, int, void* )
{
    // 如果检测到鼠标左键释放事件
    if  ( event == EVENT_LBUTTONUP )
    {
        // 检查是否可以注册
        bool is_registrable = registration.is_registrable();
        if (is_registrable)
        {
            // 获取已注册的点数
            int n_regist = registration.getNumRegist();
            // 获取需要注册的顶点编号
            int n_vertex = pts[n_regist];


            // 创建2D点
            Point2f point_2d = Point2f((float)x,(float)y);
            // 获取3D点
            Point3f point_3d = mesh.getVertex(n_vertex-1);


            // 注册点
            registration.registerPoint(point_2d, point_3d);
            // 如果达到最大注册点数，结束注册
            if( registration.getNumRegist() == registration.getNumMax() ) end_registration = true;
        }
    }
}


/**  主程序  **/
int main(int argc, char *argv[])
{
    // 调用帮助信息显示函数
    help();


    // 定义命令行参数
    const String keys =
            "{help h        |      | 打印帮助信息                                                   }"
            "{image i       |      | 输入图像的路径                                                }"
            "{model         |      | 输出yml模型的路径                                             }"
            "{mesh          |      | ply网格的路径                                                 }"
            "{keypoints k   |2000  | 检测关键点的数量(仅用于ORB)                                   }"
            "{feature       |ORB   | 特征点名称 (ORB, KAZE, AKAZE, BRISK, SIFT, SURF, BINBOOST, VGG)}"
            ;
    // 创建命令行解析器
    CommandLineParser parser(argc, argv, keys);


    // 定义默认图像路径、网格文件路径和输出文件路径以及其他参数
    string img_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/resized_IMG_3875.JPG");  // 用于注册的图像
    string ply_read_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply");          // 物体网格
    string write_path = samples::findFile("samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml");     // 输出文件
    int numKeyPoints = 2000;
    string featureName = "ORB";


    // 根据命令行提供的参数更新路径和参数值
    if (parser.has("help"))
    {
        parser.printMessage();
        return 0;
    }
    else
    {
        img_path = parser.get<string>("image").size() > 0 ? parser.get<string>("image") : img_path;
        ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path;
        write_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : write_path;
        numKeyPoints = parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints;
        featureName = parser.has("feature") ? parser.get<string>("feature") : featureName;
    }


    // 打印相关路径和参数信息
    std::cout << "输入图像: " << img_path << std::endl;
    std::cout << "CAD模型: " << ply_read_path << std::endl;
    std::cout << "输出训练文件: " << write_path << std::endl;
    std::cout << "特征点: " << featureName << std::endl;
    std::cout << "ORB关键点数量: " << numKeyPoints << std::endl;


    // 使用*.ply文件路径加载网格
    mesh.load(ply_read_path);


    // 实例化RobustMatcher类：检测器、提取器、匹配器
    RobustMatcher rmatcher;
    Ptr<Feature2D> detector, descriptor;
    // 创建特征
    createFeatures(featureName, numKeyPoints, detector, descriptor);
    // 设置特征检测器和描述子提取器
    rmatcher.setFeatureDetector(detector);
    rmatcher.setDescriptorExtractor(descriptor);


    /**  第一张图像的基本真实数据  **/


    // 创建并打开窗口  创建一个保持原图像比例且大小可调的显示窗口，窗口的名字为"MODEL REGISTRATION"
    namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO);


    // 设置鼠标事件
    setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0);


    // 打开要注册的图像
    Mat img_in = imread(img_path, IMREAD_COLOR);//从 img_path 指定的文件中以彩色方式读入图像，然后将读入的图像数据存储在 Mat 类型的 img_in 中。
    Mat img_vis; // 可视图像的副本


    // 如果读取图像失败
    if (img_in.empty()) {
        cout << "无法打开或找到图像" << endl;
        return -1;
    }


    // 设置要注册的点数
    int num_registrations = n;//8
    registration.setNumMax(num_registrations);


    // 提示用户点击箱子角落
    cout << "点击箱子角落..." << endl;
    cout << "等待..." << endl;


    // 定义一些基本颜色
    const Scalar red(0, 0, 255);
    const Scalar green(0,255,0);
    const Scalar blue(255,0,0);
    const Scalar yellow(0,255,255);


    // 循环直到所有点被注册
    while ( waitKey(30) < 0 )//如果在每30毫秒内没有接收到任何用户按键输入，那么就一直执行while循环中的代码。
    {
        // 刷新调试图像
        img_vis = img_in.clone();


        // 当前已注册的点
        vector<Point2f> list_points2d = registration.get_points2d();
        vector<Point3f> list_points3d = registration.get_points3d();


        // 绘制当前已注册的点  圆点+坐标文字
        drawPoints(img_vis, list_points2d, list_points3d, red);


        // 如果注册未完成，绘制我们要注册的3D点。
        // 如果注册已完成，跳出循环。
        if (!end_registration)
        {
            // 绘制调试文字
            int n_regist = registration.getNumRegist();
            int n_vertex = pts[n_regist];
            Point3f current_poin3d = mesh.getVertex(n_vertex-1);


            drawQuestion(img_vis, current_poin3d, green);//绘制当前3D点的信息
            drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red);//绘制计数文本 
        }
        else
        {
            // 绘制调试文字
            drawText(img_vis, "注册结束", green);
            drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green);
            break;
        }


        // 显示图像
        imshow("MODEL REGISTRATION", img_vis);
    }


    /** 计算相机位置 **/


    cout << "计算位置..." << endl;


    // 已注册点的列表
    vector<Point2f> list_points2d = registration.get_points2d();
    vector<Point3f> list_points3d = registration.get_points3d();


    // 根据已注册的点估计位置  通过使用潜在空间点(list_points3d)和2D图像空间点(list_points2d)的对应关系，对相机进行姿态估计
    bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, SOLVEPNP_ITERATIVE);
    if ( is_correspondence )
    {
        cout << "找到对应点" << endl;


        // 计算网格的所有2D点以验证算法并绘制
        vector<Point2f> list_points2d_mesh = pnp_registration.verify_points(&mesh);
        draw2DPoints(img_vis, list_points2d_mesh, green);
    }
    else {
        cout << "没有找到对应点" << endl << endl;
    }


    // 显示图像
    imshow("MODEL REGISTRATION", img_vis);


    // 等待直到按下ESC键
    waitKey(0);


    /** 计算图像关键点的3D **/
    // 模型关键点和描述子的容器
    vector<KeyPoint> keypoints_model;
    Mat descriptors;


    // 计算关键点和描述子
    rmatcher.computeKeyPoints(img_in, keypoints_model);
    rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);


    // 检查关键点是否在注册图像的表面，并添加到模型中
    //这段代码主要用于处理关键点模型中的每一个点，对于每个点，它创建
    //一个对应的2D点，并使用PnP（Perspective-n-Point）注册并尝试
    //将2D点反投影（backproject）回3D。如果反投影成功（即点在模型
    //表面上），则在模型中添加对应的2D-3D点、描述符和关键点；否则，
    //将该2D点添加到模型的离群点（outliers）中。这对于创建和调整3D模型
    //，以及进行模型匹配和识别等任务来说，非常关键。
    // 从零开始遍历关键点模型的每一个点
    for (unsigned int i = 0; i < keypoints_model.size(); ++i) {
        // 创建一个2D点，取自关键点模型的第i个点的位置
        Point2f point2d(keypoints_model[i].pt);
        // 创建一个空的3D点
        Point3f point3d;
        // 利用PnP(指视角nP点)识别并将2D点反投影到3D
        // 若点在物体表面上，on_surface则为真；否则，为假
        bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d);
        // 若该点在物体表面上
        if (on_surface)
        {
            // 在模型中添加对应的2D和3D点
            model.add_correspondence(point2d, point3d);
            // 向模型添加描述符，这是从描述符中的第i行取得的
            model.add_descriptor(descriptors.row(i));
            // 在对应模型中添加关键点，这是从关键点模型的第i个关键点取得的
            model.add_keypoint(keypoints_model[i]);
        }
        // 若该点不在物体表面上
        else
        {
            // 将该2D点添加到模型的异常值中
            model.add_outlier(point2d);
        }
    }


    // 设置训练图像路径
    model.set_trainingImagePath(img_path);
    // 保存模型到*.yaml文件
    model.save(write_path);


    // 输出图像
    img_vis = img_in.clone();


    // 模型的2D点列表
    vector<Point2f> list_points_in = model.get_points2d_in();
    vector<Point2f> list_points_out = model.get_points2d_out();


    // 绘制一些调试文本
    string num = IntToString((int)list_points_in.size());
    string text = "有 " + num + " 个内点";
    drawText(img_vis, text, green);


    // 绘制一些调试文本
    num = IntToString((int)list_points_out.size());
    text = "有 " + num + " 个外点";
    drawText2(img_vis, text, red);


    // 绘制物体网格
    drawObjectMesh(img_vis, &mesh, &pnp_registration, blue);


    // 根据是否在表面绘制找到的关键点
    draw2DPoints(img_vis, list_points_in, green);
    draw2DPoints(img_vis, list_points_out, red);


    // 显示图像
    imshow("MODEL REGISTRATION", img_vis);


    // 等待直到按下ESC键
    waitKey(0);


    // 关闭并销毁窗口
    destroyWindow("MODEL REGISTRATION");


    cout << "再见" << endl;
}

知识点：

小孔成像模型

创建PNP问题对象

加载ply网格

不同特征检测器和描述符提取器

估计相机的位姿：即相机相对于3D模型的旋转和平移

// 给定2D/3D对应点列表和使用的方法，估计姿态的函数
bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
                               const std::vector<cv::Point2f> &list_points2d,
                               int flags)
{
    // 初始化畸变系数矩阵、旋转向量和平移向量
    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;


    // 姿态估计
    bool correspondence = cv::solvePnP( list_points3d, list_points2d, A_matrix_, distCoeffs, rvec, tvec,
                                        useExtrinsicGuess, flags);


    // 将旋转向量转换为矩阵
    Rodrigues(rvec, R_matrix_);
    t_matrix_ = tvec;
    
    // 设置投影矩阵
    this->set_P_matrix(R_matrix_, t_matrix_);


    return correspondence;
}