最近正在用opencv實(shí)現(xiàn)測(cè)距，看到網(wǎng)上有完整代碼的不多，所以就來寫一篇。參考了這篇文章：
1.Opencv:SolvePNP
本文章使用的opencv版本：4.1.1
這篇博客就不說pnp的理論知識(shí)了，網(wǎng)上有很多，自認(rèn)寫不出更高級(jí)的東西來。完整的代碼在最后面，需要什么資料留言就好，我每天都會(huì)看。

1.相機(jī)標(biāo)定

想要獲得三維世界中的坐標(biāo)，首先需要對(duì)相機(jī)進(jìn)行標(biāo)定。標(biāo)定的方法可以看我的另一片博客：matlab標(biāo)定并用opencv驗(yàn)證,是用matlab進(jìn)行標(biāo)定參數(shù)的求解，因?yàn)閙atlab標(biāo)定相機(jī)不用代碼，直接圖形化界面操作，很簡(jiǎn)單。

2.測(cè)量世界坐標(biāo)

我們知道pnp問題至少需要四組解，就是說，在待測(cè)量的圖片中，我們最少要知道四個(gè)點(diǎn)的相互關(guān)系，也就是四個(gè)點(diǎn)之間的相對(duì)坐標(biāo)。下面舉例說明：
這是我待測(cè)量的圖片(圖片較大，加載不出來稍等)：

原圖.png

現(xiàn)在需要找出四個(gè)點(diǎn)，作為基準(zhǔn)。我用opencv的角點(diǎn)檢測(cè)得到四個(gè)點(diǎn)，代碼如下：

#include <iostream>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <opencv2/ml.hpp>
using namespace std;
using namespace cv;
using namespace cv::ml;
int main()
{
    Mat orignal_image = imread("/root/桌面/IMG_3354.png");
    cv::Mat gray_image , after_harris_image;
    cv::Mat norm_image; //歸一化后的圖
    cv::Mat scaled_image; //線性變換后的八位無符號(hào)整形的圖
    cv::cvtColor (orignal_image,gray_image,COLOR_BGR2GRAY);     // 灰度變換
    vector<Point2f> corners;//提供初始角點(diǎn)的坐標(biāo)位置和精確的坐標(biāo)的位置
    int maxcorners = 4;
    double qualityLevel = 0.01;  //角點(diǎn)檢測(cè)可接受的最小特征值
    double minDistance = 10;    //角點(diǎn)之間最小距離
    int blockSize = 3;//計(jì)算導(dǎo)數(shù)自相關(guān)矩陣時(shí)指定的領(lǐng)域范圍
    double  k = 0.04; //權(quán)重系數(shù)

    goodFeaturesToTrack(gray_image, corners, maxcorners, qualityLevel, minDistance, Mat(), blockSize, false, k);
    //Mat():表示感興趣區(qū)域；false:表示不用Harris角點(diǎn)檢測(cè)
    //輸出角點(diǎn)信息
    cout << "角點(diǎn)信息為：" << corners.size() << endl;
    //繪制角點(diǎn)
    for (unsigned i = 0; i < corners.size(); i++)
    {
        circle(orignal_image, corners[i], 10, cv::Scalar(10, 255, 0), -1, 8, 0);
        cout << "角點(diǎn)坐標(biāo)：" << corners[i] << endl;
    }
    //namedWindow("image",0);
    //imshow("iamge",orignal_image);
    imwrite("角點(diǎn).png",orignal_image);
    waitKey(0);
    return 0;
}

結(jié)果如下所示：

角點(diǎn).png

圖中四個(gè)綠色的點(diǎn)就是檢測(cè)到的角點(diǎn)（圖片看起來不太一樣是因?yàn)樘罅藷o法上傳，裁剪了一下），根據(jù)opencv返回的四個(gè)點(diǎn)的相機(jī)坐標(biāo)為1(1275,1968)，2(1464,2007)，3(1303,2102)，4(1187,2042)。相機(jī)坐標(biāo)是以左上角為零點(diǎn)，向右為x軸正方向，向下為y軸正方向。
接下來要測(cè)量這四個(gè)點(diǎn)之間的實(shí)際長度，經(jīng)測(cè)量，得：1(0,0)，2(12.5,2.5)，3(2.5,8)，4(-4.5,5)，以點(diǎn)1為原點(diǎn)，向右為x軸正方向，向下為y軸正方向。我這里的單位用的是mm，用什么單位最后計(jì)算的結(jié)果也是什么單位。

3.計(jì)算

3.1計(jì)算旋轉(zhuǎn)角

//計(jì)算相機(jī)旋轉(zhuǎn)角
    double theta_x, theta_y,theta_z;
    double PI = 3.14;
    theta_x = atan2(rotM.at<double>(2, 1), rotM.at<double>(2, 2));
    theta_y = atan2(-rotM.at<double>(2, 0),
    sqrt(rotM.at<double>(2, 1)*rotM.at<double>(2, 1) + rotM.at<double>(2, 2)*rotM.at<double>(2, 2)));
    theta_z = atan2(rotM.at<double>(1, 0), rotM.at<double>(0, 0));
    theta_x = theta_x * (180 / PI);
    theta_y = theta_y * (180 / PI);
    theta_z = theta_z * (180 / PI);

3.2計(jì)算深度

    //計(jì)算深度
    Mat P;
    P = (rotM.t()) * tvecs;//將旋轉(zhuǎn)向量變換為旋轉(zhuǎn)矩陣后叉乘平移向量

其中P的z軸坐標(biāo)就是深度信息。

完整代碼如下：

#include <opencv2/calib3d.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <fstream>
using namespace std;
using namespace cv;
int main(){
    //相機(jī)內(nèi)參矩陣與外參矩陣
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    cameraMatrix.at<double>(0, 0) = 3374.07818952427;
    cameraMatrix.at<double>(0, 1) = -2.78181259296951;
    cameraMatrix.at<double>(0, 2) = 2019.19661037399;
    cameraMatrix.at<double>(1, 1) = 3374.34656463011;
    cameraMatrix.at<double>(1, 2) = 1501.95020619850;
    cameraMatrix.at<double>(2, 2) = 1;

    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
    distCoeffs.at<double>(0, 0) =  0.173230511639020;
    distCoeffs.at<double>(1, 0) = -0.645138161101467;
    distCoeffs.at<double>(2, 0) = -0.00109294300160736;
    distCoeffs.at<double>(3, 0) = -3.47866401740176e-06;
    distCoeffs.at<double>(4, 0) = 0;

    //將控制點(diǎn)在世界坐標(biāo)系的坐標(biāo)壓入容器
    vector<Point3f> objP;
    objP.clear();
    objP.push_back(Point3f(0, 0, 0));
    objP.push_back(Point3f(12.5, 2.5, 0));
    objP.push_back(Point3f(2.5, 8, 0));
    objP.push_back(Point3f(-4.5, 5, 0));

    //將之前已經(jīng)檢測(cè)到的角點(diǎn)的坐標(biāo)壓入容器
    std::vector<Point2f> points;
    points.clear();
    points.push_back(Point2f(1275,1968));
    points.push_back(Point2f(1464,2007));
    points.push_back(Point2f(1303,2102));
    points.push_back(Point2f(1187,2042));

    //創(chuàng)建旋轉(zhuǎn)矩陣和平移矩陣
    Mat rvecs = Mat::zeros(3,1,CV_64FC1);
    Mat tvecs = Mat::zeros(3,1,CV_64FC1);

    //求解pnp
    solvePnP(objP, points, cameraMatrix, distCoeffs, rvecs, tvecs);
    Mat rotM = Mat::eye(3,3,CV_64F);
    Mat rotT = Mat::eye(3,3,CV_64F);
    Rodrigues(rvecs, rotM);  //將旋轉(zhuǎn)向量變換成旋轉(zhuǎn)矩陣
    Rodrigues(tvecs, rotT);

    //計(jì)算相機(jī)旋轉(zhuǎn)角
    double theta_x, theta_y,theta_z;
    double PI = 3.14;
    theta_x = atan2(rotM.at<double>(2, 1), rotM.at<double>(2, 2));
    theta_y = atan2(-rotM.at<double>(2, 0),
    sqrt(rotM.at<double>(2, 1)*rotM.at<double>(2, 1) + rotM.at<double>(2, 2)*rotM.at<double>(2, 2)));
    theta_z = atan2(rotM.at<double>(1, 0), rotM.at<double>(0, 0));
    theta_x = theta_x * (180 / PI);
    theta_y = theta_y * (180 / PI);
    theta_z = theta_z * (180 / PI);

    //計(jì)算深度
    Mat P;
    P = (rotM.t()) * tvecs;

    //輸出
    cout<<"角度"<<endl;
    cout<<theta_x<<endl;
    cout<<theta_y<<endl;
    cout<<theta_z<<endl;
    cout<<P<<endl;

    return 0;
}

有問題歡迎留言交流！