2014年6月13日金曜日

2画像からの三次元復元

5点アルゴリズムによるカメラ位置・姿勢の推定で2枚の画像を撮影したカメラの相対的な位置・姿勢情報を推定することができた。ここでは、推定された位置・姿勢情報を使って対応点の三次元情報を復元する方法を紹介する。

カメラの位置・姿勢情報、カメラの内部パラメータ、画像間での2D-2D対応から、三角測量の原理に基づいて対応点の三次元座標を推定することが出来る。OpenCVでは、この三角測量による三次元座標の計算を行うためのcv::triangulatePointsが用意されている。引数は、第1,2カメラの射影行列、それぞれの画像の対応点座標を格納した配列、三次元座標出力用の配列である。

先日の5点アルゴリズムによるカメラ位置・姿勢の推定のプログラムでcv::recoverPoseした後に、下記のコードを加えることで、対応点の三次元情報を復元することが出来る。
ただし、誤対応情報を利用する場合には、cv::findEssentialMatの部分を以下のように書き換える必要がある。

cv::Mat mask; //RANSACの結果を保持するためのマスク

cv::Mat essentialMat = cv::findEssentialMat(p1, p2, 1.0, cv::Point2f(0, 0), cv::RANSAC, 0.9999, 0.003, mask);


以下のプログラムでは、三次元座標の復元は焦点距離1.0の正規化座標系での対応点座標を用いている。なので、それぞれのカメラの内部パラメータ行列は単位行列となり、射影行列はカメラの外部パラメータ行列と一致する。
    //正規化座標系で計算しているのでProjection matrix=Extrinsic camera parameter matrix
    cv::Mat prjMat1, prjMat2;
    prjMat1 = cv::Mat::eye(3, 4, CV_64FC1); //片方は回転、並進ともに0
    prjMat2 = cv::Mat(3, 4, CV_64FC1);
    for (int i = 0; i < 3; ++i)
      for (int j = 0; j < 3; ++j)
      {
        prjMat2.at<double>(i, j) = r.at<double>(i, j);
      }
    prjMat2.at<double>(0, 3) = t.at<double>(0);
    prjMat2.at<double>(1, 3) = t.at<double>(1);
    prjMat2.at<double>(2, 3) = t.at<double>(2);

    std::cout << "Projection Matrix 1:\n" << prjMat1 << std::endl;
    std::cout << "Projection Matrix 2:\n" << prjMat2 << std::endl;

    //三角測量による三次元位置の推定
    cv::Mat point3D;
    cv::triangulatePoints(prjMat1, prjMat2, p1, p2, point3D);

    std::ofstream ofs( "points.txt" );
    for (int i = 0; i < point3D.cols; ++i)
    {
      //誤対応以外の点を保存
      if (mask.at<unsigned char>(i) > 0)
      {
        //色情報を取得
        cv::Vec3b rgb = img1.at<cv::Vec3b>( keypoints1[dmatch[i].queryIdx].pt );
        ofs << point3D.at<double>(0, i)/point3D.at<double>(3, i) << " " << point3D.at<double>(1, i)/point3D.at<double>(3, i) << " " << point3D.at<double>(2, i)/point3D.at<double>(3, i) << " " << (int)rgb[0] << " " << (int)rgb[1] << " " << (int)rgb[2] << std::endl;
      }
    }
    ofs.close();

8 件のコメント:

  1. 学校の勉強の為に本サイトを閲覧しサンプルコード参考としたものです。

    cv::recoverPose
    cv::findEssentialMat

    二つが識別子として認識されないといわれます。

    opencv2.4.11
    visual studio2015です。

    どうか改善する点がありましたら教えてください。

    宜しくお願いします。

    返信削除
    返信
    1. opencvのバージョンの問題ですので、3.0以降のバージョンでお試しください。

      削除
    2. 教えて頂きありがとうございます。

      Trianglepoint関数を使い三次元復元を行おうとしたのですが、座標が(0.8,0.9,1.2)といった不可解な結果となりました。

      事前に対応づけし、カメラはお互いに平行化したのですがうまく行きません。リファレンスに書いてある射影復元を参考にして3×4のマトリックスを表示して行ったのですが、そこが間違いの要因でしょうか。

      削除
    3. http://goya813.hatenablog.com/entry/2015/03/05/203853
      こちらの方の復元結果と非常によく似ています。

      正規化座標系であれば単位幅、単位奥行きなのでXYZ座標値は全て1以下の値として表現される為この様な結果として出力されるのでしょうか。

      削除
    4. このコメントは投稿者によって削除されました。

      削除
  2. 返信して頂きありがとうございます。

    z座標が1.2なので正規化座標系では範囲を超えているんだと思うんです。
    hbiが基準長の半分
    cameraMatrixL、cameraMatrixRは3*4のマトリックス
    焦点距離は320
    光学中心は(160,120)
    これで設定をしたのですが、理論の範囲で間違っているのでしょうか。

    float hbi = 150;
    Mat cameraMatrixL =
    (Mat_(3, 4) <<
    320, 0, 160, hbi * 320,
    0, 320, 120, 0,
    0, 0, 1, 0);

    Mat cameraMatrixR =
    (Mat_(3, 4) <<
    320, 0, 160, -hbi * 320,
    0, 320, 120, 0,
    0, 0, 1, 0);

    返信削除
    返信
    1. 焦点距離、光学中心は校正結果で値を算出したものです。

      削除
    2. 算出される三次元座標が正規化されているわけではないので、z座標は1を超えることはあります。各種座標系の関係は、この辺の資料を読むと分かると思います。
      http://www.hvrl.ics.keio.ac.jp/kisorin2013/reference/kisorin2013-2.pdf

      削除