opencv识别装甲板并用pnp计算出平移矩阵与旋转矩阵

本文链接：https://blog.csdn.net/m0_74835410/article/details/130411132

这个代码中opencv部分来源于这篇https://blog.csdn.net/weixin_64054906/article/details/126674493

应作业要求外加上pnp计算平移矩阵与旋转矩阵

#include "stdio.h"
#include <iostream> 
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;
//由于在识别中的核心物体以及相关的物理特性是灯条，所以建一个灯条类
class LightDescriptor
{	    //在识别以及匹配到灯条的功能中需要用到旋转矩形的长宽偏转角面积中心点坐标等
public:float width, length, angle, area;
      cv::Point2f center;
public:
    LightDescriptor() {};
    //让得到的灯条套上一个旋转矩形，以方便之后对角度这个特殊因素作为匹配标准
    LightDescriptor(const cv::RotatedRect& light)
    {
        width = light.size.width;
        length = light.size.height;
        center = light.center;
        angle = light.angle;
        area = light.size.area();
    }
};
 
int main()
{
    Mat camera_matrix = (Mat_<double>(3, 3) << 2.4533055557326181e+03, 0., 9.4925681914912386e+02, 0.,
                         2.4551723094234911e+03, 5.5454977746938223e+02, 0., 0., 1.);
    Mat distortion_coeffs = (Mat_<double>(1, 5) << -2.7582931901279197e-01, 1.3973322381045875e-01,
                             2.8099747483245295e-03, -1.8185597815801518e-04, 0.);
    VideoCapture video; //VC类对象化
    video.open("111.mp4");
    //变量集中定义
    Mat frame, channels[3], binary, Gaussian, dilatee;
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    Rect boundRect;
    RotatedRect box;
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    vector<Point2f> boxPts(4);
    //图像预处理
    for (;;) {
        Rect point_array[20];
        video >> frame;  //读取每帧
        if (frame.empty()) {
            break;
        }
        split(frame, channels); //通道分离
        threshold(channels[0], binary, 220, 255, 0);//二值化
        GaussianBlur(binary, Gaussian, Size(5, 5), 0);//滤波
        dilate(Gaussian, dilatee, element);
        // dilate(Gaussian, dilate, element, Point(-1, -1));//膨胀，把滤波得到的细灯条变宽
        findContours(dilatee, contours, hierarchy, RETR_TREE, CHAIN_APPROX_NONE);//轮廓检测
        vector<LightDescriptor> lightInfos;//创建一个灯条类的动态数组
    //筛选灯条
        for (int i = 0; i < contours.size(); i++) {
            // 求轮廓面积
            double area = contourArea(contours[i]);
            // 去除较小轮廓&fitEllipse的限制条件
            if (area < 5 || contours[i].size() <= 1)
                continue;//相当于就是把这段轮廓去除掉
            // 用椭圆拟合区域得到外接矩形（特殊的处理方式：因为灯条是椭圆型的，所以用椭圆去拟合轮廓，再直接获取旋转外接矩形即可）
            RotatedRect Light_Rec = fitEllipse(contours[i]);
 
            // 长宽比和轮廓面积比限制（由于要考虑灯条的远近都被识别到，所以只需要看比例即可）
            if (Light_Rec.size.width / Light_Rec.size.height > 4)
                continue;
            lightInfos.push_back(LightDescriptor(Light_Rec));
        }
        //二重循环多条件匹配灯条
        for (size_t i = 0; i < lightInfos.size(); i++) {
            for (size_t j = i + 1; (j < lightInfos.size()); j++) {
                LightDescriptor& leftLight = lightInfos[i];
                LightDescriptor& rightLight = lightInfos[j];
                float angleGap_ = abs(leftLight.angle - rightLight.angle);
                //由于灯条长度会因为远近而受到影响，所以按照比值去匹配灯条
                float LenGap_ratio = abs(leftLight.length - rightLight.length) / max(leftLight.length, rightLight.length);
                float dis = pow(pow((leftLight.center.x - rightLight.center.x), 2) + pow((leftLight.center.y - rightLight.center.y), 2), 0.5);
                //均长
                float meanLen = (leftLight.length + rightLight.length) / 2;
                float lengap_ratio = abs(leftLight.length - rightLight.length) / meanLen;
                float yGap = abs(leftLight.center.y - rightLight.center.y);
                float yGap_ratio = yGap / meanLen;
                float xGap = abs(leftLight.center.x - rightLight.center.x);
                float xGap_ratio = xGap / meanLen;
                float ratio = dis / meanLen;
                //匹配不通过的条件
                if (angleGap_ > 15 ||
                    LenGap_ratio > 1.0 ||
                    lengap_ratio > 0.8 ||
                    yGap_ratio > 1.5 ||
                    xGap_ratio > 2.2 ||
                    xGap_ratio < 0.8 ||
                    ratio > 3 ||
                    ratio < 0.8) {
                    continue;
                }
                //绘制矩形
                Point center = Point((leftLight.center.x + rightLight.center.x) / 2, (leftLight.center.y + rightLight.center.y) / 2);
                RotatedRect rect = RotatedRect(center, Size(dis, meanLen), (leftLight.angle + rightLight.angle) / 2);
                Point2f vertices[4];
                rect.points(vertices);
                for (int i = 0; i < 4; i++) {
                    line(frame, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2.2);
                }
                // 定义装甲板四个顶点的坐标
                vector<Point3f> object_points;
    object_points.push_back(Point3f(-12.5f, -5.5f, 0.f)); // 左下角
    object_points.push_back(Point3f(-12.5f, 5.5f, 0.f));  // 左上角
    object_points.push_back(Point3f(12.5f, 5.5f, 0.f));   // 右上角
    object_points.push_back(Point3f(12.5f, -5.5f, 0.f));  // 右下角
                vector<Point2f> image_points;
    image_points.push_back(vertices[0]); // 左下角
    image_points.push_back(vertices[1]);  // 左上角
    image_points.push_back(vertices[2]);  // 右上角
    image_points.push_back(vertices[3]); // 右下角
                 // 使用PnP算法计算旋转向量和平移向量
    Mat rvec, tvec;
    solvePnP(object_points, image_points, camera_matrix, distortion_coeffs, rvec, tvec);

    // 将旋转向量转化为旋转矩阵
    Mat rotation_matrix;
    Rodrigues(rvec, rotation_matrix);

    // 输出旋转矩阵和平移矩阵
    cout << "Rotation Matrix: " << endl << rotation_matrix << endl;
    cout << "Translation Vector: " << endl << tvec << endl;
            }
        }
        namedWindow("video", WINDOW_FREERATIO);
        imshow("video", frame);
        waitKey(5);
    }
    video.release();
    cv::destroyAllWindows();
    return 0;
}

1 加载相机矩阵和畸变参数

2加载图片并进行预处理

3定义装甲板四个顶点的坐标

4定义装甲板四个顶点在图像中的坐标//在这里我们直接用opencv获取的2D坐标来代替

5使用PnP算法计算旋转向量和平移向量

主要使用solvePnP函数

6将旋转向量转化为旋转矩阵

7输出旋转矩阵和平移矩阵