說明：

?在共面直線測試中，由于計算誤差等原因，共面條件判斷不準，但計算結果依然正確。

// point-position2.cpp : 定義控制臺應用程序的入口點。 #include "stdafx.h" #include <stdio.h> #include <iostream> #include "opencv2/core/core.hpp" #include "opencv2/features2d/features2d.hpp" #include "opencv2/highgui/highgui.hpp" #include <opencv2/nonfree/features2d.hpp> #include "opencv2/imgproc/imgproc.hpp" #include "opencv2/nonfree/nonfree.hpp" #include "opencv2/legacy/legacy.hpp" #include<Eigen/Core> #include <Eigen/Dense> #include<math.h> using namespace cv;int main( int argc, char** argv ) {Mat img_1 = imread("book_in_scene.png");Mat img_2 = imread("book2.png");if( !img_1.data || !img_2.data ){ std::cout<< " --(!) Error reading images " << std::endl; return -1; }//-- Step 1: Detect the keypoints using SURF Detectorint minHessian = 400;SiftFeatureDetector detector( minHessian );//SurfFeatureDetector detector( minHessian ); vector<KeyPoint> keypoints_1, keypoints_2;detector.detect( img_1, keypoints_1 );detector.detect( img_2, keypoints_2 );//-- Step 2: Calculate descriptors (feature vectors) SiftDescriptorExtractor extractor;//SurfDescriptorExtractor extractor; Mat descriptors_1, descriptors_2;extractor.compute( img_1, keypoints_1, descriptors_1 );extractor.compute( img_2, keypoints_2, descriptors_2 );//-- Step 3: Matching descriptor vectors using FLANN matcher FlannBasedMatcher matcher;std::vector< DMatch > matches;matcher.match( descriptors_1, descriptors_2, matches );double max_dist = 0; double min_dist = 100;//-- Quick calculation of max and min distances between keypointsfor( int i = 0; i < descriptors_1.rows; i++ ){ double dist = matches[i].distance;if( dist < min_dist ) min_dist = dist;if( dist > max_dist ) max_dist = dist;}//printf("-- Max dist : %f \n", max_dist );//printf("-- Min dist : %f \n", min_dist );//-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )//-- PS.- radiusMatch can also be used here.std::vector< DMatch > good_matches;for( int i = 0; i < descriptors_1.rows; i++ ){ if( matches[i].distance < 2*min_dist ){ good_matches.push_back( matches[i]); }}//-- Draw only "good" matches Mat img_matches;drawMatches( img_1, keypoints_1, img_2, keypoints_2,good_matches, img_matches);//-- Show detected matches//imshow( "Good Matches", img_matches );//imwrite("Lena_match_surf.jpg",img_matches);//imwrite("Lena_match_sift.jpg",img_matches);//good_matches[i].queryIdx保存著第一張圖片匹配點的序號，keypoints_1[good_matches[i].queryIdx].pt.x 為該序號對應的點的x坐標。y坐標同理//good_matches[i].trainIdx保存著第二張圖片匹配點的序號，keypoints_2[good_matches[i].trainIdx].pt.x 為為該序號對應的點的x坐標。y坐標同理printf( "--Keypoint 1:%f,%f: %d -- Keypoint 2:%f,%f: %d \n", keypoints_1[good_matches[0].queryIdx].pt.x,keypoints_1[good_matches[0].queryIdx].pt.y,good_matches[0].queryIdx, keypoints_2[good_matches[0].trainIdx].pt.x,keypoints_2[good_matches[0].trainIdx].pt.y,good_matches[0].trainIdx );/*_______________________________________________________________________________________________________________________________*/double x_inImage1,y_inImage1,x_inImage2,y_inImage2,y,X,Y,alpha,gamma;//像面坐標(x,y)和圖像尺寸（X，Y）以及成像視場角（alpha,gamma）double x1,y1,z1,x2,y2,z2;//雙站坐標double alpha1,gamma1;//雙站俯仰角和偏轉角double alpha2,gamma2;//賦予初始值alpha1=45;alpha1=90;//測試共面gamma1=45;alpha2=270;gamma2=45;X=640;Y=480;double FOVx=10;double FOVy=FOVx*Y/X;x1=0,y1=0,z1=0;x2=0,y2=200,z2=0;/* //測角偏差補償x_inImage1=keypoints_1[good_matches[0].queryIdx].pt.x;//目標點坐標由匹配所得y_inImage1=keypoints_1[good_matches[0].queryIdx].pt.y;x_inImage2=keypoints_2[good_matches[0].queryIdx].pt.x;y_inImage2=keypoints_2[good_matches[0].queryIdx].pt.y;double deviation_alpha1=(x_inImage1-X/2)/X*FOVx;double deviation_alpha2=(x_inImage2-X/2)/X*FOVx;double deviation_gamma1=(y_inImage1-Y/2)/X*FOVy;double deviation_gamma2=(y_inImage2-Y/2)/X*FOVy;alpha1=alpha1+deviation_alpha1;alpha2=alpha2+deviation_alpha2;gamma1=gamma1+deviation_gamma1;gamma2=gamma2+deviation_gamma2; *///開始計算double pi=16*(atan(1.0/5))-4*atan(1.0/239);//精確定義圓周率std::cout<<"pi為:"<<pi<<std::endl;alpha1=alpha1*pi/180;//角度弧度轉換gamma1=gamma1*pi/180;alpha2=alpha2*pi/180;gamma2=gamma2*pi/180;// std::cout<<"cos(alpha1)為:"<<cos(alpha1)<<std::endl; // std::cout<<"cos(gamma1)為:"<<cos(gamma1)<<std::endl;double m1=(cos(alpha1))*(cos(gamma1));double n1=(sin(alpha1))*(cos(gamma1));double p1=sin(gamma1);double m2=(cos(alpha2))*(cos(gamma2));double n2=(sin(alpha2))*(cos(gamma2));double p2=sin(gamma2);std::cout<<"方向向量1為:"<<m1<<"，"<<n1<<"，"<<p1<<std::endl;std::cout<<"方向向量2為:"<<m2<<"，"<<n2<<"，"<<p2<<std::endl;double coplane;//共面判斷coplane=(x2-x1)*(n1*p2-n2*p1)-(y2-y1)*(m1*p2-m2*p1)+(z2-z1)*(m1*n2-m2*n1);//coplane=0共面if(coplane){//計算公垂線方向向量A1、B1、C1double A1=n1*p2-n2*p1;double B1=p1*m2-p2*m1;double C1=m1*n2-m2*n1;// double A2=n2*C1-p2*B1;double B2=p2*A1-m2*C1;double C2=m2*B1-n2*A1;double A3=n1*C1-p1*B1;double B3=p1*A1-m1*C1;double C3=m1*B1-n1*A1;double delta1=n1*(B1*C2-B2*C1)+m1*(A1*C2-A2*C1);double delta2=n2*(B1*C3-B3*C1)+m2*(A1*C3-A3*C1);double D1=A2*(x2-x1)+B2*(y2-y1)+C2*(z2-z1);double D2=A3*(x1-x2)+B3*(y1-y2)+C3*(z1-z2);double Xg,Yg,Zg,Xh,Yh,Zh,Xtarget,Ytarget,Ztarget;//兩直線垂足G和H點坐標,目標點在其中點位置。Xg=x1-(D1*m1*C1)/delta1;Yg=y1-(D1*n1*C1)/delta1;Zg=z1+D1*(A1*m1+B1*n1)/delta1;Xh=x2-(D2*m2*C1)/delta2;Yh=y2-(D2*n2*C1)/delta2;Zh=z2+D2*(A1*m2+B1*n2)/delta2;Xtarget=(Xg+Xh)/2;Ytarget=(Yg+Yh)/2;Ztarget=(Zg+Zh)/2;std::cout<<"目標坐標為:"<<Xtarget<<"，"<<Ytarget<<"，"<<Ztarget<<std::endl<<std::endl;}else//兩線共面且相交，引入參數t {double t;t=(p2*(y1-y2)+n2*(z2-z1))/(n2*p1-p2*n1);double Xtarget,Ytarget,Ztarget;Xtarget=x1+m1*t;Ytarget=y1+n1*t;Ztarget=z1+p1*t;std::cout<<"目標坐標為:"<<Xtarget<<"，"<<Ytarget<<"，"<<Ztarget<<std::endl<<std::endl;}getchar();//waitKey(0);return 0; }

?

共面直線測試中，沒有跳進共面直線解析交點中，但結果依然正確：

單獨測試共面直線求交點結果為：

?

轉載于:https://www.cnblogs.com/wxl845235800/p/9067754.html

總結

以上是生活随笔為你收集整理的point-position2修改版的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。