// 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 -; }

    //-- 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 ;

}