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<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 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 \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,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;

    x1=0,y1=0,z1=0;
    x2=0,y2=200,z2=0;
    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;
    x=keypoints_1[good_matches[0].queryIdx].pt.x;//目标点坐标由匹配所得
    y=keypoints_1[good_matches[0].queryIdx].pt.y;
    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;
    Eigen::MatrixXd a1(2,2);//待求A1、B1、C1等参数
    Eigen::MatrixXd b1(2,2);
    Eigen::MatrixXd c1(2,2);
    Eigen::MatrixXd a2(2,2);
    Eigen::MatrixXd b2(2,2);
    Eigen::MatrixXd c2(2,2);
    a1<<n1,p1,n2,p2;
    b1<<p1,m1,p2,m2;
    c1<<m1,n1,m2,n2;
    double A1=a1.determinant();//计算行列式
    double B1=b1.determinant();
    double C1=c1.determinant();
    a2<<n2,B1,p2,C1;
    b2<<p2,C1,m2,A1;
    c2<<m2,A1,n2,B1;
    double A2=a2.determinant();//计算行列式
    double B2=b2.determinant();
    double C2=c2.determinant();
    
    double A3=n1*C1-p1*B1;
    double B3=p1*A1-m1*C1;
    double C3=m1*B1-n1*A1;

    Eigen::MatrixXd delta10(3,3);
    delta10<<A1,B1,C1,A2,B2,C2,n1,-m1,0;
    double delta1=delta10.determinant();
    Eigen::MatrixXd delta20(2,2);
    Eigen::MatrixXd delta21(2,2);
    delta20<<B1,C1,B3,C3;
    delta21<<A1,C1,A3,C3;
    double delta2=n2*(delta20.determinant())+m2*(delta21.determinant());
    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;

    //TP<<xw,yw,zw,1;我们要求的
    //逆矩阵、行列式     std::cout << "行列式： " << c.determinant() << std::endl;

    //输入两个探测站的俯仰角和偏转角度数，下面会转换成弧度。
    //假设值

    std::cout<<"目标坐标为:"<<Xtarget<<"，"<<Ytarget<<"，"<<Ztarget<<std::endl<<std::endl;
    getchar();
    //waitKey(0);
    return 0;
}