三角測量法による距離測定

実現したい事
ステレオカメラによる距離測定を行いたいと考えています。
そのために三角測量法を用いたいと考えているのですが、わからない点が2つほどあります。
１、計測に使用したカメラがオートフォーカスであるため物体の距離によって焦点距離が変わってしまいます。カメラ行列から焦点距離を出すことができるらしいのですがどのように表示させればよろしいでしょうか？？
２、カメラの視差を求めるのは左右画像間の差を利用するのですが、現在の表示結果を使用していいのでしょうか？？

視差マップを利用するような記事はあるのですが直接距離を計算するプログラム例がないため困っています、どなたかご教授お願いできないでしょうか？？

実行結果
特徴点を複数検出しその平均値を求め差にしています

c++
1#include <opencv2/highgui/highgui.hpp>
2#include <opencv2/imgproc/imgproc.hpp>
3#include <opencv2/features2d/features2d.hpp>
4#include <opencv2/calib3d/calib3d.hpp>
5#include <opencv2/xfeatures2d/nonfree.hpp>
6#include <iostream>
7
8using namespace cv;
9using namespace std;
10
11int main(void)
12{
13    
14    int idx[2000];
15    int x,y,i = 0;
16    int x2,y2,i2 = 0;
17    int x3,y3 = 0;
18    int x4,y4 = 0;
19    int i5 = 0;
20    double dis,ave = 0;
21    int distance = 0;
22    int i6 = 0;
23    int y5 = 0;
24    int y6 = 0;
25    
26    // 画像の読み込み
27    Mat img_src1 = imread( "left30.jpg" );
28    Mat img_src2 = imread( "right30.jpg" );
29    
30    // 画像の表示
31    imshow( "leftcamera", img_src1);
32    imshow( "rightcamera", img_src2);
33    waitKey( 0 );
34    
35    auto akaze = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0,3,0.001f);
36    
37    vector<KeyPoint> keypoints1,keypoints2;
38    akaze->detect(img_src1,keypoints1);
39    akaze->detect(img_src2,keypoints2);
40    
41    // 特徴点の座標、大きさを表示keypoint1
42    vector<KeyPoint>::iterator it = keypoints1.begin(), it_end = keypoints1.end();
43    for(;it != it_end;it++){
44        i++;
45        x = it->pt.x;
46        y = it->pt.y;
47        cout << "keypoint1 " << i << " x=" << x << " y=" << y << "\n";
48    }
49    // 特徴点の座標、大きさを表示keypoint2
50    vector<KeyPoint>::iterator it2 = keypoints2.begin(), it2_end = keypoints2.end();
51    for(;it2 != it2_end;it2++){
52        i2++;
53        x2 = it2->pt.x;
54        y2 = it2->pt.y;
55        cout << "keypoint2 " << i2 << " x=" << x2 << " y=" << y2 << "\n";
56    }
57    
58    Mat descriptor1, descriptor2;
59    akaze->compute(img_src1, keypoints1, descriptor1);
60    akaze->compute(img_src2, keypoints2, descriptor2);
61    cout << "rows横:" << descriptor1.rows << endl;
62    cout << "cols:" << descriptor1.cols << endl;
63    cout << "rows横:" << descriptor2.rows << endl;
64    cout << "cols:" << descriptor2.cols << endl;
65   
66    //DescriptorMatcherオブジェクトの生成
67    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//BruteForce-Hamming
68    // 特徴点のマッチング情報を格納する変数
69    vector<DMatch> dmatch, match;
70    // 特徴点マッチングの実行
71    matcher->match(descriptor1, descriptor2, match);
72    
73    // 最小距離の取得
74    double min_dist = DBL_MAX;//double の最大値
75    
76    for(int i = 0; i < (int)match.size(); i++){
77        double dist = match[i].distance;//画像間の特徴点距離
78        int kppp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
79        int kppp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
80       
81        cout << "distance"<< " "<< i+1 << " " << dist << "\n";
82        
83        //特徴点間の最小距離を求める
84        if(dist < min_dist){
85            min_dist = dist;
86            //特徴量の表示descriptor1
87            Mat d(descriptor1,Rect(0,kppp1_idx,descriptor1.cols,1));
88            cout << "descriptor1 " << kppp1_idx+1 << ": " << d << endl;
89            
90            //特徴量の表示descriptor2
91            Mat d2(descriptor2,Rect(0,kppp2_idx,descriptor2.cols,1));
92            cout << "descriptor2 " << kppp2_idx+1 << ": " << d2 << endl;
93            
94            cout << "min_distance" << " " << min_dist << "\n";
95        }
96    }
97    // しきい値を設定する
98    const double threshold = 2.0 * min_dist;
99    cout << "threshold"<< " "<< threshold << "\n";
100    
101    for(int i = 0; i < (int)match.size(); i++){
102        int kpp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
103        int kpp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
104        /* 画像1の対応点座標 */
105        y5 = keypoints1[kpp1_idx].pt.y;
106        /* 画像2の対応点座標 */
107        y6 = keypoints2[kpp2_idx].pt.y;
108        
109        if(match[i].distance < threshold && y5 < y6+10 && y5 > y6-10){
110            idx[i] = 1;
111            dmatch.push_back(match[i]);            
112        }else{
113            idx[i] = -1;
114        }
115    }
116    
117    for (int i = 0; i < (int)match.size(); i++){
118        int kp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
119        int kp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
120        
121        if(idx[i] != -1){
122            i5++;
123            cout << "dmatch " << i5 << " \n";
124            /* 画像1の対応点座標 */
125            x3 = keypoints1[kp1_idx].pt.x;
126            y3 = keypoints1[kp1_idx].pt.y;
127            /* 画像2の対応点座標 */
128            x4 = keypoints2[kp2_idx].pt.x;
129            y4 = keypoints2[kp2_idx].pt.y;
130            
131            //特徴量の表示descriptor1
132                Mat d(descriptor1,Rect(0,kp1_idx,descriptor1.cols,1));
133                cout << "descriptor1 " << kp1_idx+1 << ": " << d << endl;
134            
135            //特徴量の表示descriptor2
136                Mat d2(descriptor2,Rect(0,kp2_idx,descriptor2.cols,1));
137                cout << "descriptor2 " << kp2_idx+1 << ": " << d2 << endl;
138            
139            //座標表示
140            cout << "keypoint1 "<< kp1_idx+1 << " x=" << x3 << " y=" << y3 << " " << "keypoint2 "<<  kp2_idx+1 << " x=" << x4 << " y=" << y4 << "\n";
141            dis += (x3-x4);
142            ave = dis/i5;
143        }
144    }
145    cout << "x軸の特徴点座標差異の平均値は" << ave << "です\n";
146
147    Mat dest;
148    drawMatches(img_src1, keypoints1, img_src2, keypoints2, dmatch, dest,DrawMatchesFlags::DEFAULT);//DrawMatchesFlags::DEFAULT
149    
150    imshow("result",dest);
151    waitKey( 0 );
152    
153    return 0;
154    }