cv::solvePnP の使い方【OpenCV/C++】〜PnP問題を解いて姿勢推定する〜

OpenCV for C++
📌 準備: OpenCV の環境構築がまだの方はこちら → C++ 環境構築ガイド / C#(OpenCvSharp)セットアップ

cv::solvePnP は、3D空間上の点群とその2D投影点の対応から、カメラに対するオブジェクトの姿勢(回転・並進)を推定する関数です。ARマーカーの追跡、ロボットの把持位置推定、カメラ外部パラメータの推定など、幅広い場面で使われます。

最小の呼び出し形は次の通りです。

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);

動作環境

  • OpenCV 4.x
  • コンパイル例: g++ -std=c++17 main.cpp $(pkg-config --cflags --libs opencv4) -o main

環境構築がまだの方は環境構築ガイドを先にどうぞ。


基本の使い方

下記のサンプルでは、既知の3Dモデル点(例: 平面上の正方形マーカー)と、画像上で検出した2D点を使って姿勢を推定します。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>

int main()
{
    // --- カメラ内部パラメータ(事前にキャリブレーション済みの値を使う) ---
    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3)
        << 800.0,   0.0, 320.0,
              0.0, 800.0, 240.0,
              0.0,   0.0,   1.0);
    cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64F); // 歪み係数(今回はゼロ)

    // --- 3D モデル点(オブジェクト座標系): 一辺 100mm の正方形マーカー ---
    std::vector<cv::Point3f> objectPoints = {
        {  0.0f,   0.0f, 0.0f },
        {100.0f,   0.0f, 0.0f },
        {100.0f, 100.0f, 0.0f },
        {  0.0f, 100.0f, 0.0f }
    };

    // --- 対応する 2D 画像点(実際はコーナー検出等で取得する) ---
    std::vector<cv::Point2f> imagePoints = {
        {200.0f, 150.0f},
        {280.0f, 148.0f},
        {282.0f, 230.0f},
        {198.0f, 232.0f}
    };

    // --- 姿勢推定 ---
    cv::Mat rvec, tvec;
    bool success = cv::solvePnP(
        objectPoints,  // 3D 点群
        imagePoints,   // 対応する 2D 点群
        cameraMatrix,  // カメラ行列
        distCoeffs,    // 歪み係数
        rvec,          // 出力: 回転ベクトル (Rodrigues)
        tvec           // 出力: 並進ベクトル
    );

    if (!success) {
        std::cerr << "solvePnP failed." << std::endl;
        return -1;
    }

    std::cout << "rvec:\n" << rvec << std::endl;
    std::cout << "tvec:\n" << tvec << std::endl;

    // --- 回転ベクトル → 回転行列に変換 ---
    cv::Mat rotMat;
    cv::Rodrigues(rvec, rotMat);
    std::cout << "Rotation Matrix:\n" << rotMat << std::endl;

    // --- 再投影: 推定姿勢で 3D 点を 2D に投影して誤差を確認 ---
    std::vector<cv::Point2f> projectedPoints;
    cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);

    double totalError = 0.0;
    for (size_t i = 0; i < imagePoints.size(); ++i) {
        double dx = imagePoints[i].x - projectedPoints[i].x;
        double dy = imagePoints[i].y - projectedPoints[i].y;
        totalError += std::sqrt(dx * dx + dy * dy);
    }
    std::cout << "Mean reprojection error: "
              << totalError / imagePoints.size() << " px" << std::endl;

    return 0;
}

実行すると次のような出力が得られます(数値は入力点によって変わります)。

rvec:
[-0.1300466797931787;
 0.07671956791566822;
 -0.01630005051612003]
tvec:
[-146.8273271875711;
 -108.9771442966776;
 972.5245194582014]
Rotation Matrix:
[0.9969301151839882, 0.0112584840830077, 0.07748284955648284;
 -0.02121644809788654, 0.9914275797249171, 0.1289232969284559;
 -0.07536715311957574, -0.1301714281128765, 0.9886228762949524]
Mean reprojection error: 0.752465 px

各ブロックの解説

ブロック ポイント
cameraMatrix 焦点距離・主点を含む 3×3 行列。cv::calibrateCamera で事前取得が必要
objectPoints オブジェクト座標系での 3D 点。単位は mm でも m でも構わないが tvec の単位が揃う
imagePoints 画像上の対応 2D 点。歪み補正済みでなくても distCoeffs を渡せば内部で補正
rvec / tvec Rodrigues 表現の回転ベクトル(3×1)と並進ベクトル(3×1)
cv::Rodrigues rvec を 3×3 の回転行列に変換
cv::projectPoints 再投影誤差の確認に使う。0.5px 以下が実用の目安

引数と戻り値

bool cv::solvePnP(
    InputArray  objectPoints,   // 3D 点群 (N×3, CV_32F or CV_64F)
    InputArray  imagePoints,    // 2D 点群 (N×2, CV_32F or CV_64F)
    InputArray  cameraMatrix,   // カメラ行列 (3×3)
    InputArray  distCoeffs,     // 歪み係数 (4,5,8,12,14 要素)
    OutputArray rvec,           // 回転ベクトル (3×1, CV_64F)
    OutputArray tvec,           // 並進ベクトル (3×1, CV_64F)
    bool        useExtrinsicGuess = false,  // true: rvec/tvec の初期値を使う
    int         flags = cv::SOLVEPNP_ITERATIVE  // アルゴリズム選択
);
引数 型 説明
objectPoints InputArray 3D 点。最低点数はアルゴリズムに依存
imagePoints InputArray 2D 点。objectPoints と同数
cameraMatrix InputArray 焦点距離・主点を含む 3×3 内部行列
distCoeffs InputArray 歪み係数。空 cv::Mat() を渡すとゼロ扱い
rvec / tvec OutputArray 推定された姿勢(Rodrigues ベクトル / 並進)
useExtrinsicGuess bool ITERATIVE 使用時に初期値を与える場合 true
flags int アルゴリズム(下表参照)
戻り値 bool 解が得られた場合 true

flags の主な選択肢

定数 必要点数 特徴
SOLVEPNP_ITERATIVE ≥4(非平面の点群は ≥6) デフォルト。反復最適化で精度高め
SOLVEPNP_EPNP ≥4 高速。点数が多い場合に有効
SOLVEPNP_P3P 4(最初の3点使用) 最小構成解。外れ値なしで高速
SOLVEPNP_IPPE ≥4(平面点群のみ) 平面物体に特化。高精度
SOLVEPNP_IPPE_SQUARE 4(正方形マーカーの4隅を決まった順序・座標で) ArUco等の正方形マーカー向け

実践例

外れ値に強い推定: cv::solvePnPRansac

実環境では対応点に外れ値(マッチングミス等)が含まれます。cv::solvePnPRansac を使うとロバストに推定できます。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>

int main()
{
    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3)
        << 800.0,   0.0, 320.0,
              0.0, 800.0, 240.0,
              0.0,   0.0,   1.0);
    cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64F);

    // 意図的に外れ値を混ぜた対応点
    std::vector<cv::Point3f> objectPoints = {
        {  0.f,   0.f, 0.f}, {100.f,   0.f, 0.f},
        {100.f, 100.f, 0.f}, {  0.f, 100.f, 0.f},
        { 50.f,  50.f, 0.f}, { 75.f,  25.f, 0.f}
    };
    std::vector<cv::Point2f> imagePoints = {
        {200.f, 150.f}, {280.f, 148.f},
        {282.f, 230.f}, {198.f, 232.f},
        {999.f, 999.f}, // ← 外れ値
        {260.f, 169.f}
    };

    cv::Mat rvec, tvec;
    std::vector<int> inliers;

    bool success = cv::solvePnPRansac(
        objectPoints, imagePoints,
        cameraMatrix, distCoeffs,
        rvec, tvec,
        false,    // useExtrinsicGuess
        100,      // iterationsCount
        8.0f,     // reprojectionError(閾値 px)
        0.99,     // confidence
        inliers   // インライア点のインデックスが格納される
    );

    if (!success) {
        std::cerr << "solvePnPRansac failed." << std::endl;
        return -1;
    }

    std::cout << "Inliers: " << inliers.size() << " / "
              << objectPoints.size() << std::endl;
    std::cout << "tvec: " << tvec.t() << std::endl;

    return 0;
}

実行すると次のような出力が得られます。

Inliers: 5 / 6
tvec: [-147.048194208173, -109.0264106558114, 973.4163292149259]

外れ値のインデックス(今回は index=4)が除外され、残り5点でロバストな推定ができていることを確認できます。


つまずきポイント

⚠️ objectPoints / imagePoints の型不一致で assert 落ち

objectPoints に std::vector<cv::Point3f>、imagePoints に std::vector<cv::Point2f> を渡すのが最も安全です。Mat で渡す場合、objectPoints は N×3 の1チャンネル(または N×1 / 1×N の3チャンネル)、imagePoints は N×2 の1チャンネル(または N×1 / 1×N の2チャンネル)にします。行と列を入れ替えた形状や、2つの点数が一致しない場合は実行時 assertion が発生します。

// NG 例: 3×N(行と列が逆)の Mat を渡すと assertion エラー
cv::Mat pts(3, 4, CV_64F);
cv::solvePnP(pts, ...); // ← 失敗

// OK: N×3 の Mat
cv::Mat pts(4, 3, CV_64F);

⚠️ 点数が少ないとアルゴリズムによって解なし

SOLVEPNP_P3P は厳密に4点(最初の3点で解を求め4点目で検証)が必要です。4点未満では失敗します。SOLVEPNP_ITERATIVE は、点が同一平面上なら4点以上、平面上にない場合は6点以上が必要です。平面の点群には SOLVEPNP_IPPE も使えます。SOLVEPNP_IPPE_SQUARE は、正方形マーカーの4隅を (-L/2, L/2, 0)・(L/2, L/2, 0)・(L/2, -L/2, 0)・(-L/2, -L/2, 0)(L は一辺の長さ)の順で与えた場合専用で、本記事の基本例のように角を原点に置いた座標では正しい姿勢になりません。


関連する関数

  • cv::Rodrigues — rvec(回転ベクトル)と回転行列の相互変換に必須
  • cv::projectPoints — 推定姿勢による再投影誤差の確認に使う
  • cv::solvePnPRansac — 外れ値に強いロバスト版
  • cv::calibrateCamera — solvePnP に必要なカメラ内部パラメータの事前取得
  • 特徴点マッチングで 2D 対応点を取得する場面では cv::BFMatcher / FLANN の使い方【OpenCV/C++】 や cv::SIFT の使い方【OpenCV/C++】 が前段処理として役立ちます
  • 推定した姿勢を時系列で安定化させたい場合は cv::KalmanFilter の使い方【OpenCV/C++】 の組み合わせが実務でよく使われます

まとめ

cv::solvePnP は3D-2D対応点さえ揃えば数行で姿勢推定が完結します。外れ値が懸念される実環境では cv::solvePnPRansac に切り替えるだけでロバスト性が大幅に向上します。推定後は必ず cv::projectPoints で再投影誤差を確認し、0.5px 以下を目安に精度を評価してください。

🛠 画像処理のプロが開発するSDK/API

本ブログを運営するスワローインキュベートは、OpenCV ベースのなりすまし判定SDK/API(C++製・OpenCV 4.10)を開発しています。顔認証システムへの組み込み実績多数。

→ 顔認証のなりすまし対策 設計チェックリスト(当ブログ)
→ なりすまし判定SDK/APIの詳細を見る
→ API仕様書・サンプルコード

タイトルとURLをコピーしました