point-position2 初步完善版。
不再使用eigen库，行列式直接计算得出结果。判断共面异面分别处理。
先提取双站获得图像的匹配特征点，由双站位置信息解析目标位置。
// 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<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 Detector int 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 keypoints for( 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 ", max_dist ); //printf("-- Min dist : %f ", 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 ", 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; 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、C1 double 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; }