Target Dual Station location simulation C + + code

Point-position2 Preliminary Perfect version

The two stations are extracted to obtain the matching feature points of the image, and the target location is resolved by the two station position information.

//point-position2.cpp: Defines the entry point of the console application. #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 namespaceCV;intMainintargcChar**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    intMinhessian = -;    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 matcherFlannbasedmatcher Matcher; Std::vector< Dmatch >matches;    Matcher.match (Descriptors_1, descriptors_2, matches); DoubleMax_dist =0;DoubleMin_dist = -; //--Quick calculation of Max and min distances between keypoints     for(inti =0; i < descriptors_1.rows; i++ )    { DoubleDist =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 are less than 2*min_dist)//--ps.-Radiusmatch can also is used here.std::vector< Dmatch >good_matches;  for(inti =0; i < descriptors_1.rows; i++ )    { if(Matches[i].distance <2*min_dist)    {Good_matches.push_back (matches[i]);} }    //--Draw only "good" matchesMat 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 holds the ordinal of the first image match point, keypoints_1[good_matches[i].queryidx].pt.x the x-coordinate of the point corresponding to the ordinal. Y coordinate//Good_matches[i].trainidx holds the ordinal of the second picture match point, keypoints_2[good_matches[i].trainidx].pt.x the x-coordinate of the point corresponding to the ordinal. Y coordinateprintf"--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); /*___________________________________________________________________________________________________________ ____________________*/    DoubleX_inimage1,y_inimage1,x_inimage2,y_inimage2,y,x,y,alpha,gamma;//Image Polygon Coordinates (x, y) and images size (x, Y) and imaging field of view (Alpha,gamma)    DoubleX1,Y1,Z1,X2,Y2,Z2;//Dual Station coordinates    DoubleALPHA1,GAMMA1;//two-station pitch angle and deflection angle    DoubleAlpha2,gamma2; //give the initial valuealpha1= $; GAMMA1= $; ALPHA2= the; Gamma2= $; X=640; Y=480; Doublefovx=Ten; Doublefovy=fovx*y/X; X1=0, y1=0, z1=0; X2=0, y2= $, z2=0;/*//Angular deviation compensation x_inimage1=keypoints_1[good_matches[0].queryidx].pt.x;//target point coordinates are matched by the resulting Y_INIMAGE1=KEYPOINTS_1[GOOD_MATC    Hes[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;*/    //Start Calculation    DoublePi= -* (Atan (1.0/5))-4*atan (1.0/239);//precise definition of pistd::cout<<"pi is:"<<pi<<Std::endl; Alpha1=alpha1*pi/ the;//Angle radians Conversiongamma1=gamma1*pi/ the; ALPHA2=alpha2*pi/ the; Gamma2=gamma2*pi/ the;//std::cout<< "cos (ALPHA1) for:" <<cos (ALPHA1) <<std::endl;//std::cout<< "cos (GAMMA1) for:" <<cos (GAMMA1) <<std::endl;    Doublem1= (cos (ALPHA1)) *(cos (GAMMA1)); DoubleN1= (sin (alpha1)) *(cos (GAMMA1)); Doublep1=sin (gamma1); Doublem2= (cos (ALPHA2)) *(cos (GAMMA2)); DoubleN2= (sin (alpha2)) *(cos (GAMMA2)); DoubleP2=sin (gamma2); Std::cout<<"the direction vector 1 is:"<<m1<<","<<n1<<","<<p1<<Std::endl; Std::cout<<"the direction Vector 2 is:"<<m2<<","<<n2<<","<<p2<<Std::endl; DoubleCoplane;//Common face judgmentCoplane= (x2-x1) * (N1*P2-N2*P1)-(y2-y1) * (M1*P2-M2*P1) + (Z2-Z1) * (M1*N2-M2*N1);//coplane=0 co-surface    if(coplane) {//calculate crossover direction vectors A1, B1, C1        Doublea1=n1*p2-n2*P1; Doubleb1=p1*m2-p2*M1; Doublec1=m1*n2-m2*N1; //        Doublea2=n2*c1-p2*B1; Doubleb2=p2*a1-m2*C1; Doublec2=m2*b1-n2*A1; DoubleA3=n1*c1-p1*B1; DoubleB3=p1*a1-m1*C1; DoubleC3=m1*b1-n1*A1; DoubleDelta1=n1* (B1*C2-B2*C1) +m1* (a1*c2-a2*C1); Doubledelta2=n2* (B1*C3-B3*C1) +m2* (a1*c3-a3*C1); Doubled1=a2* (x2-x1) +b2* (y2-y1) +c2* (z2-Z1); Doubled2=a3* (x1-x2) +b3* (y1-y2) +c3* (z1-z2); DoubleXg,yg,zg,xh,yh,zh,xtarget,ytarget,ztarget;//two linear perpendicular g and H point coordinates, where the target point is located. 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<<"the target coordinates are:"<<Xtarget<<","<<Ytarget<<","<<Ztarget<<std::endl<<Std::endl; }    Else//two lines coplanar and intersecting, introducing parameter T    {        DoubleT; T= (p2* (y1-y2) +n2* (Z2-Z1))/(n2*p1-p2*N1); DoubleXtarget,ytarget,ztarget; Xtarget=x1+m1*T; Ytarget=y1+n1*T; Ztarget=z1+p1*T; Std::cout<<"the target coordinates are:"<<Xtarget<<","<<Ytarget<<","<<Ztarget<<std::endl<<Std::endl;    } getchar (); //waitkey (0);    return 0;}

Target Dual Station location simulation C + + code