だいぶ時間が空きましたが、引き続きOpenCVの3次元復元系の関数を見ていく。
今回はcalib3dモジュールにあるtriangulatePoints関数。つまり三角測量を行う関数ですね。
1 2 3 4 5 6 |
void cv::triangulatePoints(InputArray projMatr1, InputArray projMatr2, InputArray projPoints1, InputArray projPoints2, OutputArray points4D ) |
三角測量で点を再構築します。
パラメータ
- projMatr1 1つ目のカメラの射影行列(3×4)
- projMatr2 2つ目のカメラの射影行列(3×4)
- projPoints1 1枚目の画像中の特徴点の配列(2xN)
- projPoints2 2枚目の画像中の1枚目に対応する特徴点の配列(2xN)
- points4D 同次座標における再構成後の点の配列(4×N)
この関数は、ステレオカメラによる観測によって(同次座標での)3次元点を再構築します。投影行列はstereoRectify関数で得ることができます。
注意
この関数を使うには、すべての入力データがfloat型である必要があります。
http://docs.opencv.org/3.2.0/d9/d0c/group__calib3d.html#gad3fc9a0c82b08df034234979960b778c
スポンサーリンク
具体的な使い方を見て行こう。こちらのブログ記事に載っているコードを参考に書いてみた。↓
使う2枚の画像は、以前撮影したスターデストロイヤーのターンテーブル動画から1フレーム目と50フレーム目を抜粋して使用する。
撮影に使ったiPhone6sのカメラキャリブレーションデータもあるし。
以下がソースコード。AKAZE特徴で2枚の画像の対応点を求めることにした。
結果の可視化にvizモジュールを使っているので、vizも含めてビルド済みのOpenCV3.3.0-rcを使用。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 |
#include <vector> #include <iostream> #include <opencv2/opencv.hpp> int main(int argc, char** argv) { // カメラのキャリブレーションデータ読み込み cv::Mat cameraMatrix, distCoeffs; cv::FileStorage fs("cameraParam.xml", cv::FileStorage::READ); if (fs.isOpened()) { fs["cameraMatrix"] >> cameraMatrix; fs["distCoeffs"] >> distCoeffs; } else { return -1; } cv::Mat img1; //入力画像1 cv::Mat img2; //入力画像2 // 画像ファイルを読み込み、レンズの歪み補正 cv::undistort(cv::imread("./images/starDestroyer_0001.png"), img1, cameraMatrix, distCoeffs); cv::undistort(cv::imread("./images/starDestroyer_0050.png"), img2, cameraMatrix, distCoeffs); // AKAZE特徴抽出 cv::Ptr<cv::FeatureDetector> detector = cv::AKAZE::create(); // 検出器 cv::Ptr<cv::DescriptorExtractor> descriptorExtractor = cv::AKAZE::create();// 特徴量 cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce"); // 対応点探索方法の設定 std::vector<cv::KeyPoint> keypoints1, keypoints2; std::vector<cv::Scalar> keyPointsColor1, keyPointsColor2; cv::Mat descriptor1, descriptor2; detector->detect(img1, keypoints1); descriptorExtractor->compute(img1, keypoints1, descriptor1); detector->detect(img2, keypoints2); descriptorExtractor->compute(img2, keypoints2, descriptor2); //対応点の探索 std::vector<cv::DMatch> dmatch; std::vector<cv::DMatch> dmatch12, dmatch21; matcher->match(descriptor1, descriptor2, dmatch12); //img1 -> img2 matcher->match(descriptor2, descriptor1, dmatch21); //img2 -> img1 for (size_t i = 0; i < dmatch12.size(); i++) { //img1 -> img2 と img2 -> img1の結果が一致しているか検証 cv::DMatch m12 = dmatch12[i]; cv::DMatch m21 = dmatch21[m12.trainIdx]; if (m21.trainIdx == m12.queryIdx) {// 一致しているものだけを抜粋 dmatch.push_back(m12); } } cv::Mat matchImage; //マッチング結果を描画 cv::drawMatches(img1, keypoints1, img2, keypoints2, dmatch, matchImage); cv::imshow("Matches", matchImage); if (dmatch.size() > 5) {//十分な数の対応点があれば基礎行列を推定する std::vector<cv::Point2d> p1; std::vector<cv::Point2d> p2; //対応付いた特徴点の取り出しと焦点距離1.0のときの座標に変換 for (size_t i = 0; i < dmatch.size(); i++) { cv::Mat ip(3, 1, CV_64FC1); cv::Point2d p; ip.at<double>(0) = keypoints1[dmatch[i].queryIdx].pt.x; ip.at<double>(1) = keypoints1[dmatch[i].queryIdx].pt.y; ip.at<double>(2) = 1.0; ip = cameraMatrix.inv()*ip; p.x = ip.at<double>(0); p.y = ip.at<double>(1); p1.push_back(p); ip.at<double>(0) = keypoints2[dmatch[i].trainIdx].pt.x; ip.at<double>(1) = keypoints2[dmatch[i].trainIdx].pt.y; ip.at<double>(2) = 1.0; ip = cameraMatrix.inv()*ip; p.x = ip.at<double>(0); p.y = ip.at<double>(1); p2.push_back(p); } cv::Mat mask; //RANSACの結果を保持するためのマスク cv::Mat essentialMat = cv::findEssentialMat(p1, p2, 1.0, cv::Point2f(0, 0), cv::RANSAC, 0.9999, 0.003, mask); cv::Mat r, t; cv::recoverPose(essentialMat, p1, p2, r, t); //正規化座標系で計算しているので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); cv::Mat point3D;//三角測量による三次元位置の推定 cv::triangulatePoints(prjMat1, prjMat2, p1, p2, point3D); // viz用に並べ替え cv::Mat pointCloud(point3D.cols, 1, CV_32FC3); for (int i = 0; i < point3D.cols; i++) { for (int j = 0; j < 3; j++) { pointCloud.at<cv::Vec3f>(i)[j] = point3D.at<double>(j, i); } } // 表示用vizウィンドウ作成 cv::viz::Viz3d visualizeWindow("3D"); // 点群の描画 cv::viz::WCloud cloud(pointCloud); visualizeWindow.showWidget("CLOUD", cloud); visualizeWindow.spin();// 3D Viewの描画 } return 0; } |
AKAZE特徴のマッチング結果はこちら↓
で、三角測量の結果をvizモジュールで可視化したものがこちら↓
側面は何となくそれっぽいぞ。と思って上から見てみたら、なんだか湾曲している。。。
レンズっぽい球面な歪み方だな。
あ、以前作った連番はすでにレンズの歪み補正を終えているから、補正処理は必要なかったか。
追記:よくよく考えたら、関数の出力は同次座標だからそのまま3次元ベクトルに代入して表示しちゃダメだな。
スポンサーリンク
関連記事
Also published on Medium.