[OpenCV3]ステレオカメラの校正結果よりcv::triangulatePointsを使用して３次元点群を取得したい。

Question

[OpenCV3]ステレオカメラの校正結果よりcv::triangulatePointsを使用して３次元点群を取得したい。

受付中

回答 1

投稿 2017/06/26 11:08 ・編集 2017/06/26 11:15

評価
クリップ 2
VIEW 2,941

satohiromu

score 14

前提・実現したいこと

「前提」

OpenCv3の勉強中で2次元点群から3次元復元をcv::triangulatePointsを用いて行いたい。
ステレオキャリブレーション（校正）はサンプルソースを元に実装している。
cv::projectPointsのパラメータが不明確。
下記URLのソースを参考。
http://ishidate.my.coocan.jp/opencv_22/opencv_22.htm

「実現したいこと」

OpenCV3でステレオキャリブレーションを行う。
理想的な３次元点群から画面表示（2次元座標への変換）
変換するためのパラメータに校正した結果（カメラマトリクス）を使用する。
変換した２次元座標を３次元点群へ変換
変換するためのパラメータに校正した結果（不明確）を使用する。
理想的な３次元点群と（３）で変換した３次元点群がほぼ等しいことを確認する。

発生している問題・エラーメッセージ

下記のソースコーの標準出力で
座標位置（X, Y, Z）以下と、座標位置（X, Y, Z）2以下の出力が異なる。

該当のソースコード

int main(int argc, char **argv) {

    // 省略

    // 校正
    cv::stereoCalibrate(objectPoints, imagePoints, imagePoints, K1, D1, K2, D2, size, R, T, E, F,
        CV_CALIB_FIX_ASPECT_RATIO |
        CV_CALIB_ZERO_TANGENT_DIST |
        CV_CALIB_SAME_FOCAL_LENGTH |
        CV_CALIB_RATIONAL_MODEL |
        CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5);

    stereoRectify(K1, D1, K2, D2, size, R, T, R1, R2, P1, P2, Q);
    cv::initUndistortRectifyMap(K1, D1, R1, P1, size, CV_32FC1, mapx[0], mapy[0]);

    // 理想的な３次元点群
    vector<Point3f> points3D;
    points3D.push_back(Point3f(-100, -100, -100));
    points3D.push_back(Point3f(100, -100, -100));
    points3D.push_back(Point3f(100, -100, 100));
    points3D.push_back(Point3f(-100, -100, 100));
    points3D.push_back(Point3f(-100, 100, -100));
    points3D.push_back(Point3f(100, 100, -100));
    points3D.push_back(Point3f(100, 100, 100));
    points3D.push_back(Point3f(-100, 100, 100));

    float hbl = 150, distance = 800; // Harf Base Line = 150, 奥行 = 800
    Mat cameraMatrix = (Mat_<float>(3, 3) <<
        320, 0, 160,
        0, 320, 120,
        0, 0, 1);
    Mat distCoeffs = (Mat_<float>(5, 1) << 0, 0, 0, 0, 0); // 歪み無し

    Mat rvec = (Mat_<float>(3, 1) << 0, 0, 0); // 回転無し
    Mat tvecL = (Mat_<float>(3, 1) << hbl, 0, distance);
    Mat tvecR = (Mat_<float>(3, 1) << -hbl, 0, distance);

    // L
    vector<Point2f> imagePointsL;
    cv::projectPoints(points3D, rvec, tvecL, cameraMatrix, distCoeffs, imagePointsL);
    cout << "imagePointsL" << endl;
    cout << imagePointsL << endl;

    Mat dst_imageL = Mat(Size(320, 240), CV_8UC3, Scalar::all(255));
    for (int i = 0; i < 8; i++)
        circle(dst_imageL, imagePointsL[i], 2, Scalar::all(0), -1);
    for (int i = 0; i < 4; i++) {
        line(dst_imageL, imagePointsL[i], imagePointsL[(i + 1) % 4], Scalar::all(0));
        line(dst_imageL, imagePointsL[i + 4], imagePointsL[(i + 1) % 4 + 4], Scalar::all(0));
        line(dst_imageL, imagePointsL[i], imagePointsL[i + 4], Scalar::all(0));
    }
    imshow("dst_imageL", dst_imageL);

    // R
    vector<Point2f> imagePointsR;
    cv::projectPoints(points3D, rvec, tvecR, cameraMatrix, distCoeffs, imagePointsR);
    cout << "imagePointsR" << endl;
    cout << imagePointsR << endl;

    Mat dst_imageR = Mat(Size(320, 240), CV_8UC3, Scalar::all(255));
    for (int i = 0; i < 8; i++)
        circle(dst_imageR, imagePointsR[i], 2, Scalar::all(0), -1);
    for (int i = 0; i < 4; i++) {
        line(dst_imageR, imagePointsR[i], imagePointsR[(i + 1) % 4], Scalar::all(0));
        line(dst_imageR, imagePointsR[i + 4], imagePointsR[(i + 1) % 4 + 4], Scalar::all(0));
        line(dst_imageR, imagePointsR[i], imagePointsR[i + 4], Scalar::all(0));
    }
    imshow("dst_imageR", dst_imageR);

    // camera L
    vector<Point2f> imagePointsL2;
    cv::projectPoints(points3D, R1, tvecL, K1, distCoeffs, imagePointsL2);
    cout << "imagePointsL2" << endl;
    cout << imagePointsL2 << endl;

    Mat dst_imageL2 = Mat(Size(1920, 1080), CV_8UC3, Scalar::all(255));
    for (int i = 0; i < 8; i++)
        circle(dst_imageL2, imagePointsL2[i], 2, Scalar::all(0), -1);
    for (int i = 0; i < 4; i++) {
        line(dst_imageL2, imagePointsL2[i], imagePointsL2[(i + 1) % 4], Scalar::all(0));
        line(dst_imageL2, imagePointsL2[i + 4], imagePointsL2[(i + 1) % 4 + 4], Scalar::all(0));
        line(dst_imageL2, imagePointsL2[i], imagePointsL2[i + 4], Scalar::all(0));
    }
    imshow("dst_imageL2", dst_imageL2);

    // camera R
    vector<Point2f> imagePointsR2;
    cv::projectPoints(points3D, R2, tvecR, K2, distCoeffs, imagePointsR2);
    cout << "imagePointsR2" << endl;
    cout << imagePointsR2 << endl;

    Mat dst_imageR2 = Mat(Size(1920, 1080), CV_8UC3, Scalar::all(255));
    for (int i = 0; i < 8; i++)
        circle(dst_imageR, imagePointsR2[i], 2, Scalar::all(0), -1);
    for (int i = 0; i < 4; i++) {
        line(dst_imageR2, imagePointsR2[i], imagePointsR2[(i + 1) % 4], Scalar::all(0));
        line(dst_imageR2, imagePointsR2[i + 4], imagePointsR2[(i + 1) % 4 + 4], Scalar::all(0));
        line(dst_imageR2, imagePointsR2[i], imagePointsR2[i + 4], Scalar::all(0));
    }
    imshow("dst_imageR2", dst_imageR2);

    // 3次元点群座標を取得する。
    Mat projectionMatrixL = (Mat_<float>(3, 4) << // f = 320, cx = 160, cy = 120
        320, 0, 160, hbl * 320,
        0, 320, 120, 0,
        0, 0, 1, 0);
    Mat projectionMatrixR = (Mat_<float>(3, 4) <<
        320, 0, 160, -hbl * 320,
        0, 320, 120, 0,
        0, 0, 1, 0);
    Mat points4D;
    vector<Point3f> points3D_result;
    vector<Point3f> points3D_result_list;
    cout << "座標位置（X, Y, Z）" << endl;
    for (int i = 0; i < 8; i++) {
        triangulatePoints(projectionMatrixL, projectionMatrixR,
            Mat(imagePointsL[i]), Mat(imagePointsR[i]), points4D);
        convertPointsFromHomogeneous(points4D.reshape(4, 1), points3D_result);
        cout << points3D_result << endl;

        points3D_result_list.push_back(points3D_result[0]);
    }

    Mat tvec = (Mat_<float>(3, 1) << 0, 0, distance);
    vector<Point2f> imagePoints_ret;
    cv::projectPoints(points3D_result_list, rvec, tvec, cameraMatrix, distCoeffs, imagePoints_ret);

    cout << "imagePoints_ret" << endl;
    cout << imagePoints_ret << endl;

    Mat dst_image = Mat(Size(320, 240), CV_8UC3, Scalar::all(255));
    for (int i = 0; i < 8; i++)
        circle(dst_image, imagePoints_ret[i], 2, Scalar::all(0), -1);
    for (int i = 0; i < 4; i++) {
        line(dst_image, imagePoints_ret[i], imagePoints_ret[(i + 1) % 4], Scalar::all(0));
        line(dst_image, imagePoints_ret[i + 4], imagePoints_ret[(i + 1) % 4 + 4], Scalar::all(0));
        line(dst_image, imagePoints_ret[i], imagePoints_ret[i + 4], Scalar::all(0));
    }
    imshow("dst_image", dst_image);

    // camera
    Mat points4D2;
    vector<Point3f> points3D_result2;
    vector<Point3f> points3D_result_list2;

    cout << "座標位置（X, Y, Z）2" << endl;
    for (int i = 0; i < 8; i++) {
        // triangulatePointsの第一、二引数不明
        triangulatePoints(P1, P2,
            Mat(imagePointsL2[i]), Mat(imagePointsR2[i]), points4D2); // points4D2の内容が想定外
        convertPointsFromHomogeneous(points4D2.reshape(4, 1), points3D_result2);
        cout << points3D_result2 << endl;

        points3D_result_list2.push_back(points3D_result2[0]);
    }

    Mat tvec2 = (Mat_<float>(3, 1) << 0, 0, distance);
    vector<Point2f> imagePoints_ret2;
    cv::projectPoints(points3D_result_list2, rvec, tvec2, K1, distCoeffs, imagePoints_ret2);

    cout << "imagePoints_ret2" << endl;
    cout << imagePoints_ret2 << endl;

    Mat dst_image2 = Mat(Size(1920, 1080), CV_8UC3, Scalar::all(255));
    for (int i = 0; i < 8; i++)
        circle(dst_image2, imagePoints_ret2[i], 2, Scalar::all(0), -1);
    for (int i = 0; i < 4; i++) {
        line(dst_image2, imagePoints_ret2[i], imagePoints_ret2[(i + 1) % 4], Scalar::all(0));
        line(dst_image2, imagePoints_ret2[i + 4], imagePoints_ret2[(i + 1) % 4 + 4], Scalar::all(0));
        line(dst_image2, imagePoints_ret2[i], imagePoints_ret2[i + 4], Scalar::all(0));
    }
    imshow("dst_image2", dst_image2);
}

補足情報(言語/FW/ツール等のバージョンなど)

Windows10 x64
Visual Studio 2017
C++
OpenCv3.2

気になる質問をクリップする

クリップした質問は、後からいつでもマイページで確認できます。

またクリップした質問に回答があった際、通知やメールを受け取ることができます。

クリップを取り消します
良い質問の評価を上げる

以下のような質問は評価を上げましょう
- 質問内容が明確
- 自分も答えを知りたい
- 質問者以外のユーザにも役立つ
評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。
質問の評価を上げたことを取り消します

評価を下げられる数の上限に達しました

評価を下げることができません
- 1日5回まで評価を下げられます
- 1日に1ユーザに対して2回まで評価を下げられます
質問の評価を下げる

teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。
- プログラミングに関係のない質問
- やってほしいことだけを記載した丸投げの質問
- 問題・課題が含まれていない質問
- 意図的に内容が抹消された質問
- 過去に投稿した質問と同じ内容の質問
- 広告と受け取られるような投稿
評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。
質問の評価を下げたことを取り消します

この機能は開放されていません

評価を下げる条件を満たしてません

評価を下げる理由を選択してください
プログラミングに関係のない質問やってほしいことだけを記載した丸投げの質問問題・課題が含まれていない質問意図的に内容が抹消された質問過去に投稿した質問と同じ内容の質問広告と受け取られるような投稿
詳細な説明はこちら

上記に当てはまらず、質問内容が明確になっていない質問には「情報の追加・修正依頼」機能からコメントをしてください。
質問の評価を下げる機能の利用条件

この機能を利用するためには、以下の事項を行う必要があります。
- 質問回答など一定の行動
- メールアドレスの認証
  メールアドレスの認証
- 質問評価に関するヘルプページの閲覧
  質問評価に関するヘルプページの閲覧

回答 1 件

評価が高い順
新着順
古い順

同じタグがついた質問を見る

C++
OpenCV

score 1 · Answer 1 · 2017-07-12T20:20

+1

’ステレオカメラの校正結果’より、実際の関数の動きと棄却するデータ処理の方が重要です。

ソースを読む限り、”石立氏”のHPからの参考に見えます。

facebookで”石立氏”は友人登録を受け付けておられますので、登録して質問なさられては？

投稿 2017/07/12 20:20

回答の評価を上げる

以下のような回答は評価を上げましょう
- 正しい回答
- わかりやすい回答
- ためになる回答
評価が高い回答ほどページの上位に表示されます。
回答の評価を下げる

下記のような回答は推奨されていません。
- 間違っている回答
- 質問の回答になっていない投稿
- スパムや攻撃的な表現を用いた投稿
評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

2017/07/13 09:07

貴重なご意見ありがとうございます。
有識者に直接聞く方法は目から鱗でした。

貴重なご意見ありがとうございます。有識者に直接聞く方法は目から鱗でした。

キャンセル

質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう！

[OpenCV3]ステレオカメラの校正結果よりcv::triangulatePointsを使用して３次元点群を取得したい。

前提・実現したいこと

発生している問題・エラーメッセージ

該当のソースコード

補足情報(言語/FW/ツール等のバージョンなど)

関連した質問

同じタグがついた質問を見る

質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう！

[OpenCV3]ステレオカメラの校正結果よりcv::triangulatePointsを使用して３次元点群を取得したい。

前提・実現したいこと

発生している問題・エラーメッセージ

該当のソースコード

補足情報(言語/FW/ツール等のバージョンなど)

15分調べてもわからないことは、teratailで質問しよう！

関連した質問

同じタグがついた質問を見る

SNSアカウントでログイン

teratailアカウントでログイン