3D ビジョン (6): PnP 問題 (視点と点)

PnP 問題は、既知の 3D 点 (x、y、z) とカメラ上のその投影 (u、v) を参照し、カメラのポーズ変換 R、T を解きます。
射影方程式は次のように表すことができます。
ここに画像の説明を挿入ここで、K は既知のカメラの内部パラメータ行列です。私たちがしなければならないことは、そのような 2D-3D 対応関係の n 組からカメラのポーズ変換、つまり回転行列 R と並進ベクトル t を復元することです。

記事ディレクトリ

3D ビジョン (6): PnP 問題 (視点と点)
1. アルゴリズム原理
2. 実験プロセス
3. ソースコード
4. プロジェクトリンク

1. アルゴリズム原理

P3P、直接線形変換 DLT、EPnP、UPnP、非線形バンドル調整などの典型的な PnP 問題を解決する方法は数多くあります。以下は、直接線形変換 DLT の原理の簡単な導出です。

同次座標が P=(X, Y, Z, 1).T であるある空間点 P を画像に投影し、特徴点 x1=(u1, v1, 1).T を求めることを考えます。拡張行列 [R|t] を 3*4 行列として定義し、モデルの数式は次のようになります。

ここに画像の説明を挿入
最後の行でを削除すると、次の 2 つの制約が生じます。

ここに画像の説明を挿入
表現を簡略化するために、T の行ベクトルを定義します。

ここに画像の説明を挿入
次に、上記の 2 つの制約を行列形式に変換できます。

ここに画像の説明を挿入
各特徴点は、回転行列 T と平行移動行列 T に 2 つの線形制約を提供できることがわかります。合計 N 個の特徴点があると仮定すると、次の線形方程式をリストできます。

ここに画像の説明を挿入
回転・平行移動行列 T は合計 12 次元であるため、行列 T の線形解は少なくとも 6 組の対応点によって実現でき、この方法は DLT と呼ばれます。一致する点が 6 ペアを超える場合、SVD などの方法を使用して、過剰決定方程式の最小二乗解を見つけることもできます。

2. 実験プロセス

顔のキーポイントの 2D 画像座標と 3D 顔テンプレートのキーポイント座標を使用して、頭のポーズを解決します。

顔の 2D キーポイントの画像座標は次のとおりです。

ここに画像の説明を挿入

3D 顔テンプレートのキーポイントの 3D 座標は次のとおりです。

ここに画像の説明を挿入
cv::solvePnP 関数を使用して、ポーズ変換の結果を解決します。

ここに画像の説明を挿入
頭のポーズの視覚化効果は次のとおりです。

ここに画像の説明を挿入

3. ソースコード

#include <opencv2/opencv.hpp>


using namespace std;
using namespace cv;


// reference: https://learnopencv.com/head-pose-estimation-using-opencv-and-dlib/


int main(int argc, char **argv)
{
    
    
    
    // Read input image
    cv::Mat im = cv::imread("../headPose.jpg");
    cout << "img cols and rows: " << im.cols << "  " << im.rows << endl;
    
    // 2D image points coordinate. If you change the image, you need to change vector
    std::vector<cv::Point2d> image_points;
    image_points.push_back( cv::Point2d(359, 391) );    // Nose tip
    image_points.push_back( cv::Point2d(399, 561) );    // Chin
    image_points.push_back( cv::Point2d(337, 297) );    // Left eye left corner
    image_points.push_back( cv::Point2d(513, 301) );    // Right eye right corner
    image_points.push_back( cv::Point2d(345, 465) );    // Left Mouth corner
    image_points.push_back( cv::Point2d(453, 469) );    // Right mouth corner
    
    // 3D model points coordinate.
    std::vector<cv::Point3d> model_points;
    model_points.push_back(cv::Point3d(0.0f, 0.0f, 0.0f));               // Nose tip
    model_points.push_back(cv::Point3d(0.0f, -330.0f, -65.0f));          // Chin
    model_points.push_back(cv::Point3d(-225.0f, 170.0f, -135.0f));       // Left eye left corner
    model_points.push_back(cv::Point3d(225.0f, 170.0f, -135.0f));        // Right eye right corner
    model_points.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f));      // Left Mouth corner
    model_points.push_back(cv::Point3d(150.0f, -150.0f, -125.0f));       // Right mouth corner
    
    // Camera internals parameter matrix.
    // Approximate focal length.
    // Assuming no lens distortion.
    double focal_length = im.cols; 
    Point2d center = cv::Point2d(im.cols/2, im.rows/2);
    cv::Mat camera_matrix = (cv::Mat_<double>(3,3) << focal_length, 0, center.x, 0 , focal_length, center.y, 0, 0, 1);
    cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type); 
    
    cout << endl << "Approximate Camera Matrix: " << endl << camera_matrix << endl;
    cout << endl << "Approximate Distort Coeffs: " << endl << dist_coeffs.t() << endl << endl;
    
    // Output rotation and translation, Rotation is in axis-angle form and matrix form.
    cv::Mat rotation_vector; 
    cv::Mat rotation_matrix; 
    cv::Mat translation_vector;
    
    // Solve for pose.
    // The output result of cv::solvepnp function is a rotation vector, which needs to be converted into a matrix by Rodrigues formula.
    cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);
    cv::Rodrigues(rotation_vector, rotation_matrix);
    cout << "Rotation Vector: " << endl << rotation_vector << endl << endl;
    cout << "Rotation Matrix: " << endl << rotation_matrix << endl << endl;
    cout << "Translation Vector:" << endl << translation_vector << endl << endl;
    
    // Project a 3D point (0, 0, 1000.0) onto the image plane, we use this to draw a line sticking out of the nose.
    vector<Point3d> nose_end_point3D;
    vector<Point2d> nose_end_point2D;
    nose_end_point3D.push_back(Point3d(0,0,1000.0));
    
    projectPoints(nose_end_point3D, rotation_vector, translation_vector, camera_matrix, dist_coeffs, nose_end_point2D);
    cout << "project results: " << nose_end_point2D << endl << endl;
    
    // Draw landmark points and projecting line
    for(int i=0; i < image_points.size(); i++)
    {
    
    
        circle(im, image_points[i], 3, Scalar(0, 255, 255), -1);
    }
    
    cv::line(im,image_points[0], nose_end_point2D[0], cv::Scalar(0, 0, 255), 3);
    
    // Display image.
    cv::imshow("im", im);
    cv::waitKey(0);
    cv::imwrite("../result.png", im);

}

4. プロジェクトリンク

コードが機能しない場合、またはデータセットを直接使用したい場合は、プロジェクトリンクをダウンロードできます:
https://blog.csdn.net/Twilight737