Workaround for the instable cv2.solvePnP with flags=cv2.SOLVEPNP_ITERATIVE

顔認識でFacelandmarkやHeadposeを扱う場合、cv2.solvePnP() を使って回転ベクトル (rvec) と並進ベクトル (tvec) を取得し、cv2.Rodrigues(rvec) から回転行列を求めるというのは常套手段ですが、cv2.solvePnP() の結果が安定しなくて困ったときの回避策を見つけたのでご紹介します。

やりたかったこと

顔認識で2D Facelandmarkが取得できている状態で、3D Facelandmark model と比較して頭の姿勢（いわゆるHeadpose）を求めるときに、cv2.solvePnP() はよく使われます。こんな感じですね。

ret, rvec, tvec = cv2.solvePnP(3D facelandmark model points, Estimated 2D facelandmark, camera_matrix, dist_coefficients, flags)

flagsとしては、cv2.SOLVEPNP_ITERATIVE, cv2.SOLVEPNP_P3P, cv2.SOLVEPNP_EPNP, cv2.SOLVEPNP_DLS, cv2.SOLVEPNP_UPNP などが用意されています。デフォルトは cv2.SOLVEPNP_ITERATIVE なのですが、その名の通りiterationを行い収束させていくものです。それ以外はiterationしないようです。

ここで今回の背景として、動画でリアルタイムにHeadposeを取得したいというのがありました。OpenFaceだと時間軸方向にもスムージングを行ってくれていると思われ、かなり安定したFacelandmarkが得られますが、それと似たようなことをもう少しお手軽にやりたかったという動機です。

幸い2D Facelandmarkは別途かなり安定したものが得られていたので、cv2.solvePnP() が再現性よく動作してくれればいい線行けると思ったのですが、結構ガタガタで残念な状況でした。

発生した問題

flags=cv2.SOLVEPNP_ITERATIVE を用いた場合

比較的 Estimated 2D facelandmark にマッチした3D facelandmark が再構成できるような回転ベクトルおよび並進ベクトルが出力されますが、この Estimated 3D facelandmark に対して並進ベクトルを引きさらに回転ベクトルから構成した回転行列の逆行列を掛けると、本来は 3D facelandmark model points が得られるはずなのに、ときどき上下逆さまになったものが得られてしまう…

いわゆるジンバルロック問題なのか、異常時はcv2.solvePnP() が出力する回転ベクトルのroll成分がπに近づいてはいました。（とは言っても3.0前後程度なのですが…）

グッディーのケースだと、まいどのLSTMに適用したくて、しかもシーケンスの最初の 3D Facelandmark を原点に時系列の相対位置を並べたりしたかったので、回転行列とその逆行列の両方を頻繁に使うことになり、上記のように逆さまになってしまうのは困りものでした。

それ以外の flags を用いた場合

逆さまになる問題は一切発生しないものの、Estimated 2D facelandmark に対して再構成した 3D facelandmark の精度が悪かったり、連続するフレームでかなり違う推定値を出力し時系列でみると結果がガタガタしてしまうことが散見されました。

よい回避策がないかとサーチしてみると、結構困っている人は多そうでしたね。まさに同じ問題提起もありましたが、答えは皆無と… 要は、flags=cv2.SOLVEPNP_ITERATIVE で local minima に収束しないよう、連続するフレームで毎回同じような収束過程に制限できればよいわけです。cv2.solvePnP() は回転ベクトルと並進ベクトルの計算に初期値を持たせることができるようなので、これをうまく使えないかとやってみたところ…

回避策

今のところうまく動作している回避策が見つかりましたので、ご紹介します。流れとしては以下の通りです。

• 初回はたとえば flags=cv2.SOLVEPNP_UPNP などを指定し、精度はそこそこだが安定した rvec, tvec を取得する
• 2回目以降は、flags=cv2.SOLVEPNP_ITERATIVE とし、さらに取得済みの rvec, tvec を初期値に設定し、useExtrinsicGuess=Trueを指定して、安定した収束過程で精度の良い rvec, tvec を取得する

該当部分のソースコードはこんな感じ。

if loop==0:
    ret, rvec, tvec = cv2.solvePnP(3D facelandmark model points, Estimated 2D facelandmark, camera_matrix, dist_coefficients, flags=cv2.SOLVEPNP_UPNP)
else:
    ret, rvec, tvec = cv2.solvePnP(3D facelandmark model points, Estimated 2D facelandmark, camera_matrix, dist_coefficients, rvec=rvec_last, tvec=tvec_last, useExtrinsicGuess=True, flags=cv2.SOLVEPNP_ITERATIVE)

rvec_last, tvec_last = rvec, tvec

さいごに

この回避策、顔がひとつの動画ではかなり安定して動作しています。万能な回避策ではないかも知れませんが、特定のケースではかなり使えるものだと思います。

cv2.solvePnP() の flags=cv2.SOLVEPNP_ITERATIVE の不安定性でお困りの方は、是非お試し下さい !

関連記事