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();