常連

**実現したい事**
ステレオカメラによる距離測定を行いたいと考えています。
そのために三角測量法を用いたいと考えているのですが、わからない点が2つほどあります。
１、計測に使用したカメラがオートフォーカスであるため物体の距離によって焦点距離が変わってしまいます。カメラ行列から焦点距離を出すことができるらしいのですがどのように表示させればよろしいでしょうか？？
２、カメラの視差を求めるのは左右画像間の差を利用するのですが、現在の表示結果を使用していいのでしょうか？？

視差マップを利用するような記事はあるのですが直接距離を計算するプログラム例がないため困っています、どなたかご教授お願いできないでしょうか？？

**実行結果**
特徴点を複数検出しその平均値を求め差にしています
![イメージ説明](278a388b34ddc26b7a7d5beebc7ad4d3.png)


```c++
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <iostream>

using namespace cv;
using namespace std;

int main(void)
{
    
    int idx[2000];
    int x,y,i = 0;
    int x2,y2,i2 = 0;
    int x3,y3 = 0;
    int x4,y4 = 0;
    int i5 = 0;
    double dis,ave = 0;
    int distance = 0;
    int i6 = 0;
    int y5 = 0;
    int y6 = 0;
    
    // 画像の読み込み
    Mat img_src1 = imread( "left30.jpg" );
    Mat img_src2 = imread( "right30.jpg" );
    
    // 画像の表示
    imshow( "leftcamera", img_src1);
    imshow( "rightcamera", img_src2);
    waitKey( 0 );
    
    auto akaze = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0,3,0.001f);
    
    vector<KeyPoint> keypoints1,keypoints2;
    akaze->detect(img_src1,keypoints1);
    akaze->detect(img_src2,keypoints2);
    
    // 特徴点の座標、大きさを表示keypoint1
    vector<KeyPoint>::iterator it = keypoints1.begin(), it_end = keypoints1.end();
    for(;it != it_end;it++){
        i++;
        x = it->pt.x;
        y = it->pt.y;
        cout << "keypoint1 " << i << " x=" << x << " y=" << y << "\n";
    }
    // 特徴点の座標、大きさを表示keypoint2
    vector<KeyPoint>::iterator it2 = keypoints2.begin(), it2_end = keypoints2.end();
    for(;it2 != it2_end;it2++){
        i2++;
        x2 = it2->pt.x;
        y2 = it2->pt.y;
        cout << "keypoint2 " << i2 << " x=" << x2 << " y=" << y2 << "\n";
    }
    
    Mat descriptor1, descriptor2;
    akaze->compute(img_src1, keypoints1, descriptor1);
    akaze->compute(img_src2, keypoints2, descriptor2);
    cout << "rows横:" << descriptor1.rows << endl;
    cout << "cols:" << descriptor1.cols << endl;
    cout << "rows横:" << descriptor2.rows << endl;
    cout << "cols:" << descriptor2.cols << endl;
   
    //DescriptorMatcherオブジェクトの生成
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//BruteForce-Hamming
    // 特徴点のマッチング情報を格納する変数
    vector<DMatch> dmatch, match;
    // 特徴点マッチングの実行
    matcher->match(descriptor1, descriptor2, match);
    
    // 最小距離の取得
    double min_dist = DBL_MAX;//double の最大値
    
    for(int i = 0; i < (int)match.size(); i++){
        double dist = match[i].distance;//画像間の特徴点距離
        int kppp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
        int kppp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
       
        cout << "distance"<< " "<< i+1 << " " << dist << "\n";
        
        //特徴点間の最小距離を求める
        if(dist < min_dist){
            min_dist = dist;
            //特徴量の表示descriptor1
            Mat d(descriptor1,Rect(0,kppp1_idx,descriptor1.cols,1));
            cout << "descriptor1 " << kppp1_idx+1 << ": " << d << endl;
            
            //特徴量の表示descriptor2
            Mat d2(descriptor2,Rect(0,kppp2_idx,descriptor2.cols,1));
            cout << "descriptor2 " << kppp2_idx+1 << ": " << d2 << endl;
            
            cout << "min_distance" << " " << min_dist << "\n";
        }
    }
    // しきい値を設定する
    const double threshold = 2.0 * min_dist;
    cout << "threshold"<< " "<< threshold << "\n";
    
    for(int i = 0; i < (int)match.size(); i++){
        int kpp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
        int kpp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
        /* 画像1の対応点座標 */
        y5 = keypoints1[kpp1_idx].pt.y;
        /* 画像2の対応点座標 */
        y6 = keypoints2[kpp2_idx].pt.y;
        
        if(match[i].distance < threshold && y5 < y6+10 && y5 > y6-10){
            idx[i] = 1;
            dmatch.push_back(match[i]);            
        }else{
            idx[i] = -1;
        }
    }
    
    for (int i = 0; i < (int)match.size(); i++){
        int kp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
        int kp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
        
        if(idx[i] != -1){
            i5++;
            cout << "dmatch " << i5 << " \n";
            /* 画像1の対応点座標 */
            x3 = keypoints1[kp1_idx].pt.x;
            y3 = keypoints1[kp1_idx].pt.y;
            /* 画像2の対応点座標 */
            x4 = keypoints2[kp2_idx].pt.x;
            y4 = keypoints2[kp2_idx].pt.y;
            
            //特徴量の表示descriptor1
                Mat d(descriptor1,Rect(0,kp1_idx,descriptor1.cols,1));
                cout << "descriptor1 " << kp1_idx+1 << ": " << d << endl;
            
            //特徴量の表示descriptor2
                Mat d2(descriptor2,Rect(0,kp2_idx,descriptor2.cols,1));
                cout << "descriptor2 " << kp2_idx+1 << ": " << d2 << endl;
            
            //座標表示
            cout << "keypoint1 "<< kp1_idx+1 << " x=" << x3 << " y=" << y3 << " " << "keypoint2 "<<  kp2_idx+1 << " x=" << x4 << " y=" << y4 << "\n";
            dis += (x3-x4);
            ave = dis/i5;
        }
    }
    cout << "x軸の特徴点座標差異の平均値は" << ave << "です\n";

    Mat dest;
    drawMatches(img_src1, keypoints1, img_src2, keypoints2, dmatch, dest,DrawMatchesFlags::DEFAULT);//DrawMatchesFlags::DEFAULT
    
    imshow("result",dest);
    waitKey( 0 );
    
    return 0;
    }
```

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

OpenCV 距離測定

**実現したい事**
ステレオカメラによる距離測定を行いたいと考えています。
そのために三角測量法を用いたいと考えているのですが、わからない点が2つほどあります。
１、計測に使用したカメラがオートフォーカスであるため物体の距離によって焦点距離が変わってしまいます。カメラ行列から焦点距離を出すことができるらしいのですがどのように表示させればよろしいでしょうか？？
２、カメラの視差を求めるのは左右画像間の差を利用するのですが、現在の表示結果を使用していいのでしょうか？？

視差マップを利用するような記事はあるのですが直接距離を計算するプログラム例がないため困っています、どなたかご教授お願いできないでしょうか？？

**実行結果**
特徴点を複数検出しその平均値を求め差にしています
![イメージ説明](278a388b34ddc26b7a7d5beebc7ad4d3.png)


```c++
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <iostream>

using namespace cv;
using namespace std;

int main(void)
{
    
    int idx[2000];
    int x,y,i = 0;
    int x2,y2,i2 = 0;
    int x3,y3 = 0;
    int x4,y4 = 0;
    int i5 = 0;
    double dis,ave = 0;
    int distance = 0;
    int i6 = 0;
    int y5 = 0;
    int y6 = 0;
    
    // 画像の読み込み
    Mat img_src1 = imread( "left30.jpg" );
    Mat img_src2 = imread( "right30.jpg" );
    
    // 画像の表示
    imshow( "leftcamera", img_src1);
    imshow( "rightcamera", img_src2);
    waitKey( 0 );
    
    auto akaze = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0,3,0.001f);
    
    vector<KeyPoint> keypoints1,keypoints2;
    akaze->detect(img_src1,keypoints1);
    akaze->detect(img_src2,keypoints2);
    
    // 特徴点の座標、大きさを表示keypoint1
    vector<KeyPoint>::iterator it = keypoints1.begin(), it_end = keypoints1.end();
    for(;it != it_end;it++){
        i++;
        x = it->pt.x;
        y = it->pt.y;
        cout << "keypoint1 " << i << " x=" << x << " y=" << y << "\n";
    }
    // 特徴点の座標、大きさを表示keypoint2
    vector<KeyPoint>::iterator it2 = keypoints2.begin(), it2_end = keypoints2.end();
    for(;it2 != it2_end;it2++){
        i2++;
        x2 = it2->pt.x;
        y2 = it2->pt.y;
        cout << "keypoint2 " << i2 << " x=" << x2 << " y=" << y2 << "\n";
    }
    
    Mat descriptor1, descriptor2;
    akaze->compute(img_src1, keypoints1, descriptor1);
    akaze->compute(img_src2, keypoints2, descriptor2);
    cout << "rows横:" << descriptor1.rows << endl;
    cout << "cols:" << descriptor1.cols << endl;
    cout << "rows横:" << descriptor2.rows << endl;
    cout << "cols:" << descriptor2.cols << endl;
   
    //DescriptorMatcherオブジェクトの生成
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//BruteForce-Hamming
    // 特徴点のマッチング情報を格納する変数
    vector<DMatch> dmatch, match;
    // 特徴点マッチングの実行
    matcher->match(descriptor1, descriptor2, match);
    
    // 最小距離の取得
    double min_dist = DBL_MAX;//double の最大値
    
    for(int i = 0; i < (int)match.size(); i++){
        double dist = match[i].distance;//画像間の特徴点距離
        int kppp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
        int kppp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
       
        cout << "distance"<< " "<< i+1 << " " << dist << "\n";
        
        //特徴点間の最小距離を求める
        if(dist < min_dist){
            min_dist = dist;
            //特徴量の表示descriptor1
            Mat d(descriptor1,Rect(0,kppp1_idx,descriptor1.cols,1));
            cout << "descriptor1 " << kppp1_idx+1 << ": " << d << endl;
            
            //特徴量の表示descriptor2
            Mat d2(descriptor2,Rect(0,kppp2_idx,descriptor2.cols,1));
            cout << "descriptor2 " << kppp2_idx+1 << ": " << d2 << endl;
            
            cout << "min_distance" << " " << min_dist << "\n";
        }
    }
    // しきい値を設定する
    const double threshold = 2.0 * min_dist;
    cout << "threshold"<< " "<< threshold << "\n";
    
    for(int i = 0; i < (int)match.size(); i++){
        int kpp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
        int kpp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
        /* 画像1の対応点座標 */
        y5 = keypoints1[kpp1_idx].pt.y;
        /* 画像2の対応点座標 */
        y6 = keypoints2[kpp2_idx].pt.y;
        
        if(match[i].distance < threshold && y5 < y6+10 && y5 > y6-10){
            idx[i] = 1;
            dmatch.push_back(match[i]);            
        }else{
            idx[i] = -1;
        }
    }
    
    for (int i = 0; i < (int)match.size(); i++){
        int kp1_idx = match[i].queryIdx;  // keypoint1 のインデックス
        int kp2_idx = match[i].trainIdx;  // keypoint2 のインデックス
        
        if(idx[i] != -1){
            i5++;
            cout << "dmatch " << i5 << " \n";
            /* 画像1の対応点座標 */
            x3 = keypoints1[kp1_idx].pt.x;
            y3 = keypoints1[kp1_idx].pt.y;
            /* 画像2の対応点座標 */
            x4 = keypoints2[kp2_idx].pt.x;
            y4 = keypoints2[kp2_idx].pt.y;
            
            //特徴量の表示descriptor1
                Mat d(descriptor1,Rect(0,kp1_idx,descriptor1.cols,1));
                cout << "descriptor1 " << kp1_idx+1 << ": " << d << endl;
            
            //特徴量の表示descriptor2
                Mat d2(descriptor2,Rect(0,kp2_idx,descriptor2.cols,1));
                cout << "descriptor2 " << kp2_idx+1 << ": " << d2 << endl;
            
            //座標表示
            cout << "keypoint1 "<< kp1_idx+1 << " x=" << x3 << " y=" << y3 << " " << "keypoint2 "<<  kp2_idx+1 << " x=" << x4 << " y=" << y4 << "\n";
            dis += (x3-x4);
            ave = dis/i5;
        }
    }
    cout << "x軸の特徴点座標差異の平均値は" << ave << "です\n";

    Mat dest;
    drawMatches(img_src1, keypoints1, img_src2, keypoints2, dmatch, dest,DrawMatchesFlags::DEFAULT);//DrawMatchesFlags::DEFAULT
    
    imshow("result",dest);
    waitKey( 0 );
    
    return 0;
    }

	@@ -1 +1 @@
1	- ~~OpenCV~~ 距離測定
1	+ 三角測量法による距離測定

@@ -333,3 +333,5 @@
     return 0;
     }
+```

質問編集履歴