说明：

在共面直线测试中，由于计算误差等原因，共面条件判断不准，但计算结果依然正确。

// point-position2.cpp : 定义控制台应用程序的入口点。
#include "stdafx.h"
#include
#include
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/legacy/legacy.hpp"
#include
#include
#include
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 -; }

//-- Step 1: Detect the keypoints using SURF Detector  
int minHessian = ;

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 = ; double min\_dist = ;

//-- Quick calculation of max and min distances between keypoints  
for( int i = ; 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 = ; i < descriptors\_1.rows; i++ )  
{ if( matches\[i\].distance < \*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\[\].queryIdx\].pt.x,keypoints\_1\[good\_matches\[\].queryIdx\].pt.y,good\_matches\[\].queryIdx,  
    keypoints\_2\[good\_matches\[\].trainIdx\].pt.x,keypoints\_2\[good\_matches\[\].trainIdx\].pt.y,good\_matches\[\].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=;  
alpha1=;//测试共面  
gamma1=;  
alpha2=;  
gamma2=;

X=;  
Y=;  
double FOVx=;  
double FOVy=FOVx\*Y/X;  
x1=,y1=,z1=;  
x2=,y2=,z2=;

/* //测角偏差补偿
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=*(atan(1.0/))-*atan(1.0/);//精确定义圆周率
std::cout<<"pi为:"<<pi<<std::endl;
alpha1=alpha1*pi/;//角度弧度转换
gamma1=gamma1*pi/;
alpha2=alpha2*pi/;
gamma2=gamma2*pi/;

// 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)/;  
    Ytarget=(Yg+Yh)/;  
    Ztarget=(Zg+Zh)/;

    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 ;  

}

共面直线测试中，没有跳进共面直线解析交点中，但结果依然正确：

单独测试共面直线求交点结果为：