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

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

ただいまの
回答率

88.37%

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

受付中

回答 1

投稿 編集

  • 評価
  • クリップ 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

「実現したいこと」

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

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

下記のソースコーの標準出力で
座標位置(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]);

    // 理想的な3次元点群
    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

+1

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

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

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

投稿

  • 回答の評価を上げる

    以下のような回答は評価を上げましょう

    • 正しい回答
    • わかりやすい回答
    • ためになる回答

    評価が高い回答ほどページの上位に表示されます。

  • 回答の評価を下げる

    下記のような回答は推奨されていません。

    • 間違っている回答
    • 質問の回答になっていない投稿
    • スパムや攻撃的な表現を用いた投稿

    評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

  • 2017/07/13 09:07

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

    キャンセル

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

  • ただいまの回答率 88.37%
  • 質問をまとめることで、思考を整理して素早く解決
  • テンプレート機能で、簡単に質問をまとめられる

関連した質問

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

  • トップ
  • C++に関する質問
  • [OpenCV3]ステレオカメラの校正結果よりcv::triangulatePointsを使用して3次元点群を取得したい。