OpenCVの三角測量関数『cv::triangulatepoints』

だいぶ時間が空きましたが、引き続きOpenCVの3次元復元系の関数を見ていく。

OpenCVのfindEssentialMat関数を使ったサンプルを読んでみる

本来なら例の書籍を読み進めて少しずつ理解していくつもりだったけど、しんどい。。。書籍はなかなか読み進められなかったので、飛ばし読みしてプログラムを動かしながら徐々にディティールを理解していく方針へ切り替えた(笑)何度か格闘するも上...

今回はcalib3dモジュールにあるtriangulatePoints関数。つまり三角測量を行う関数ですね。

void cv::triangulatePoints(InputArray   projMatr1,
                    InputArray  projMatr2,
                    InputArray  projPoints1,
                    InputArray  projPoints2,
                    OutputArray     points4D 
                    )

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

具体的な使い方を見て行こう。こちらのブログ記事に載っているコードを参考に書いてみた。↓
http://moitkfm.blogspot.jp/2014/06/2.html

使う2枚の画像は、以前撮影したスターデストロイヤーのターンテーブル動画から1フレーム目と50フレーム目を抜粋して使用する。

OpenCVでiPhone6sのカメラレンズ歪み補正 (マスキング)

撮影に使ったiPhone6sのカメラキャリブレーションデータもあるし。

OpenCVでiPhone6sのカメラをキャリブレーションする

世間ではiPhone7が発売されていますが、オイラは引き続きiPhone6sを使います(笑) 1年前にiPhone6Sへ機種変更してからiPhone6sのカメラで結構たくさん写真や動画を撮ったので、画像・動画がだいぶ溜まった。それ...

以下がソースコード。AKAZE特徴で2枚の画像の対応点を求めることにした。
結果の可視化にvizモジュールを使っているので、vizも含めてビルド済みのOpenCV3.3.0-rcを使用。

#include <vector>
#include <iostream>
#include <<a href="https://blog.negativemind.com/2014/05/08/opencv/">opencv</a>2/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);


    // <span class='wp_keywordlink_affiliate'><a href="https://blog.negativemind.com/tag/akaze/" title="View all posts in AKAZE" target="_blank">AKAZE</a></span>特徴抽出
    cv::Ptr<cv::FeatureDetector> detector = cv::<a href="https://blog.negativemind.com/tag/akaze/">AKAZE</a>::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;//<span class='wp_keywordlink_affiliate'><a href="https://blog.negativemind.com/tag/triangulation/" title="View all posts in 三角測量" target="_blank">三角測量</a></span>による三次元位置の推定
        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;
}

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次元ベクトルに代入して表示しちゃダメだな。