基于pnp解算的距離角度讀取，完整代碼如下，小白不懂有錯請聯(lián)系我，馬上去改

#include <opencv2/opencv.hpp> //頭文件
#include <opencv2/imgproc/imgproc.hpp>
#include
using namespace cv; //包含cv命名空間
using namespace std;
Mat getfram(VideoCapture capture)
{
Mat frame;
do {capture>>frame;}
while(frame.empty());
return frame;
}
class CalibrateDepth
{
private:
//內(nèi)參矩陣
Mat camMatrix = (Mat_(3, 3) <<917.557,0,317.0381,0,922.5312,249.2986 ,0, 0, 1);
//畸變參數(shù)
Mat distCoeff = (Mat_(5, 1) << -0.4436,0.4328,0,0,-0.1925);
public:
CalibrateDepth(vector rect_Point)
{//裝甲板參數(shù)
vector armorPoint3D;
armorPoint3D.emplace_back(Point3f(-31.5f,-32.0f,0));
armorPoint3D.emplace_back(Point3f(-31.5f,+32.0f,0));
armorPoint3D.emplace_back(Point3f(+31.5f,+32.0f,0));
armorPoint3D.emplace_back(Point3f(+31.5f,-32.0f,0));
Mat rotateMatrix;
Mat transMatrix;
solvePnP(armorPoint3D,rect_Point,camMatrix,distCoeff,rotateMatrix,transMatrix);
//double rm[3][3];
//Mat rotMat(3,3,CV_64FC1,rm);
//Rodrigues(rotateMatrix,rotMat);
//float theta_z=atan2(rm[1][0],rm[0][0]*57.2958);
//float theta_y=atan2(-rm[2][0],sqrt(rm[2][0]*rm[2][0]+rm[2][2]rm[2][2]))57.2958;
//float theta_x=atan2(rm[2][1],rm[2][2])57.2958;
//vector reference_Image;
//projectPoints(armorPoint3D,rect_Point,camMatrix,distCoeff,rotateMatrix,transMatrix,reference_Image);
//line(rectImage, reference_Image[0], reference_Image[(1)], (0,0,255), 1, 8);
double tx=transMatrix.ptr(0)[0];
double ty=transMatrix.ptr(0)[1];
double tz=transMatrix.ptr(0)[2];
double depth;
depth=sqrt(txtx+tyty+tztz);
cout<<depth<<endl;
double pitAngle=atan2(ty,tz)*57.2958;
double yawAngle=atan2(tx,tz)*57.2958;

};};

int main()
{
VideoCapture capture;
capture.open(1);
if (!capture.isOpened())
return -1;
while(1)
{
Mat srcImage,blurImage,grayImage,thresholdImage;
srcImage=getfram(capture);
imshow(“srcImage”,srcImage);
cvtColor(srcImage,grayImage,COLOR_RGB2GRAY);
blur(grayImage,grayImage,Size(9,9));
Mat Erode_element=getStructuringElement(MORPH_RECT,Size(5,5));
erode(grayImage,grayImage,Erode_element);
threshold(grayImage,thresholdImage,40,255,THRESH_BINARY);
imshow(“thresholdImage”,thresholdImage);
vector<vector> contours;
vector hierachay;
Mat contoursImage=Mat::zeros(srcImage.size(),CV_8U);
Mat rectImage=Mat::zeros(srcImage.size(),CV_8U);
vector all_rotatedrect;
Point2f rectPoint[4];
vector rect_Point;
findContours(thresholdImage,contours,hierachay,RETR_CCOMP,CHAIN_APPROX_SIMPLE);
for(const auto& contour : contours)
{
RotatedRect lightRec = minAreaRect(contour);
all_rotatedrect.emplace_back(lightRec);
}
for(int i=0;i<(int)contours.size();i++)
{
drawContours(contoursImage,contours,i,Scalar(255,255,255),1,8);
Point2f rect_points[4];all_rotatedrect[i].points(rect_points);
int q=0;
for (int j = 0; j < 4; j++)
{

line(rectImage, rect_points[j], rect_points[(j + 1) % 4], (255,255,255), 1, 8);if(q<4){rectPoint[q]=rect_points[j];q++;}}}Point2f lu, ld, ru, rd;sort(rectPoint, rectPoint + 4, [](const Point2f & p1, const Point2f & p2) { return p1.x < p2.x; });if (rectPoint[0].y < rectPoint[1].y){lu = rectPoint[0];ld = rectPoint[1];}else{lu = rectPoint[1];ld = rectPoint[0];}if (rectPoint[2].y < rectPoint[3].y) {ru = rectPoint[2];rd = rectPoint[3];}else {ru = rectPoint[3];rd = rectPoint[2];}rectPoint[0]=lu;rectPoint[1]=ld;rectPoint[2]=rd;rectPoint[3]=ru;Mat jkjl=Mat::zeros(srcImage.size(),CV_8U);for (int j = 0; j < 3; j++){ line(jkjl, rectPoint[j], rectPoint[(j + 1) % 4], (255,255,255), 1, 8);}imshow("dsj",jkjl);

//cout<<rectPoint[0]<<endl<<rectPoint[1]<<endl<<rectPoint[3]<<endl<<rectPoint[4]<<endl<<endl;
for(int i=0;i<4;i++)
rect_Point.emplace_back(rectPoint[i]);
CalibrateDepth calibrate_depth(rect_Point);
rect_Point.clear();
imshow(“contoursImage”,contoursImage);
imshow(“rectImage”,rectImage);
waitKey(30);
char c=waitKey(20);
if(c==‘q’)
break;
}
}

總結(jié)

以上是生活随笔為你收集整理的小白 初步识别读取距离的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。