欧州出張1日目 チューリッヒ編

不覚にも2月になって仕事がバタバタし始めてしまい...ブログの更新が滞ってしまいました.
個人的には仕事なんかよりロボットのほうがいろんな意味で数倍大切なんですが(笑),やっぱり仕事が詰まってくると仕事してしまいますね....サラリーマンの悲しき性なんだと思います...

で,表題のとおりヨーロッパに弾丸出張に行ってきました!以前,今の会社に入る前にドイツに半年ぐらい出向していて,「もうヨーロッパはおなか一杯...」だったのですが,たまに行くのはそれはそれで悪くないですね.

久々の成田です!乗った飛行機は,最安のスイス航空ですが..
f:id:rkoichi2001:20180223220813j:plain

成田からのチューリッヒです.英語発音すると「チューリッヒ」じゃなくて「ズーリック」ですが,外国人と話しているときに「ズーリック」って発音している自分に毎回酔っているのは,ここだけの話です(笑)
f:id:rkoichi2001:20180223221701j:plain

以前にも,ズーリック(笑)で飛行機を乗り換えたことはあったんですが,今回は宿泊しました.ついでに街を散策しましたが,地震がないせいか建物が古い&美しいですね.
f:id:rkoichi2001:20180223221505j:plain

チューリッヒ中央駅です.
f:id:rkoichi2001:20180223221940j:plain

宿泊したチューリッヒのホテルです.
f:id:rkoichi2001:20180223222141j:plain

チューリッヒの街並み@ナイトです.
f:id:rkoichi2001:20180223222408j:plain

夕食にはチーズフォンデュを食べました.チーズはとてもおいしかったんですが,ディップするものが「パン」「ポテト」だけなんですよね...もうちょっといろいろあればもっと楽しめると思うんですけどね.
f:id:rkoichi2001:20180223222643j:plain

仕事(観光?)を終えて,チューリヒからベルリンへ移動です.
f:id:rkoichi2001:20180223223539j:plain

というこで,次はベルリン編です!

カメラ位置・姿勢推定0 外部パラメータ推定実験0

下記のエントリで透視投影モデル,およびその計算式を導入し,
daily-tech.hatenablog.com

下記のエントリでPNP問題とその解法を導入しました.
daily-tech.hatenablog.com

今回のエントリとその次のエントリで実際に PNP 問題を解いてみたいと思います.両方のエントリで同じ問題「1.世界座標が既知の7点を,位置が既知のカメラに投影した時の画像座標を求める.」と「2.世界座標,画像座標が既知の点7点をつかってカメラの位置を求める」を扱いますが,今回のエントリでは OpenCV の関数を用いて,次回のエントリでは自作のプログラムで問題を解きます.

0.問題のイメージ図

f:id:rkoichi2001:20180202063605j:plain

1.世界座標が既知の7点を,位置が既知のカメラに投影した時の画像座標を求める.

ここでは,投影画像座標を求めます.問題を簡単にするために,画像にレンズひずみはないものとし,内部パラメータは下記のように定義します.
画像幅 :640pix
画像高さ:480pix
主点x  :320
主点y  :240
焦点距離:160pix
カメラ高さ:1m
カメラ角度:俯角30度

ここでは,投影点を求めるために,OpenCV の projectPoint 関数を使います.下記, Pythonスクリプトです.
ここで注意が必要なのは,R, t は「カメラ座標から見たワールド座標の位置」でなければいけないことと,オイラー角表現を用いているので回転行列の掛け算が Rx * Ry * Rz になってます.

import numpy as np
import cv2
import math

world = np.array(\
[\
( 5.00, -1.00,  0.00), \
( 5.00,  1.00,  0.00), \
( 5.00,  1.00,  2.00), \
( 5.00,  2.00,  2.00), \
( 5.00,  0.00,  3.00), \
( 5.00, -2.00,  2.00), \
( 5.00, -1.00,  2.00), \
])

# Camera Extrinsic Parameter
xRadEuler_C2W = -120 / 180 * math.pi
yRadEuler_C2W = 0 / 180 * math.pi
zRadEuler_C2W = -90 / 180 * math.pi

Rx = np.matrix([[1, 0, 0], [0, math.cos(xRadEuler_C2W), -math.sin(xRadEuler_C2W)], [0, math.sin(xRadEuler_C2W),  math.cos(xRadEuler_C2W)]])
Ry = np.matrix([[ math.cos(yRadEuler_C2W), 0, math.sin(yRadEuler_C2W)], [0, 1, 0], [-math.sin(yRadEuler_C2W), 0, math.cos(yRadEuler_C2W)]])
Rz = np.matrix([[ math.cos(zRadEuler_C2W), -math.sin(zRadEuler_C2W), 0], [ math.sin(zRadEuler_C2W),  math.cos(zRadEuler_C2W), 0], [0, 0, 1]])

# Notice : Rotation Matrix from Euler Angle.
R = Rx * Ry * Rz

# tvec is expressed wrt camra coord.
tvec = R * np.matrix((0, 0, -1)).T

# Camera Intrinsic Paramter
dist_coeffs = np.zeros((5, 1))
width = 640
height = 480
focal_length = 160
center = (width / 2, height / 2)

camera_matrix = np.array([[focal_length, 0, center[0]], 
                                           [0, focal_length, center[1]],
                                           [0, 0, 1]], dtype = "double")

if __name__ == "__main__":
  print("Calculate Projected Point")

  print("\nExtrinsic Paramter")
  print("Translation")
  print(tvec)
  
  print("Rotation")
  print(R)
  

  print("\nCamera Matrix : ")
  print(camera_matrix)

  print("\nDistortion Coefficient")
  print(dist_coeffs)
  
  print("\nGenerate Rotation Vector from Rotation Matrix")
  rvec, jacob = cv2.Rodrigues(R)
  print(rvec)
  
  print("\nProject Point on Screen")
  result = cv2.projectPoints(world, rvec, tvec, camera_matrix, None)
  
  for n in range(len(world)):
    print(world[n], '==>', result[0][n])

上記スクリプトを実行すると,下記の結果(画像座標での7点の投影位置)が得られます.

Project Point on Screen
[ 5. -1.  0.] ==> [[ 286.87457713  294.12609945]]
[ 5.  1.  0.] ==> [[ 353.12542287  294.12609945]]
[ 5.  1.  2.] ==> [[ 361.77407152  380.61258594]]
[ 5.  2.  2.] ==> [[ 403.54814303  380.61258594]]
[ 5.  0.  3.] ==> [[ 320.          443.33402461]]
[ 5. -2.  2.] ==> [[ 236.45185697  380.61258594]]
[ 5. -1.  2.] ==> [[ 278.22592848  380.61258594]]

2.世界座標,画像座標が既知の点7点をつかってカメラの位置を求める

今度は逆に,上記の結果を使ってカメラ位置を求めます.

import numpy as np
import cv2
import math

world = np.array(\
[\
( 5.00, -1.00,  0.00), \
( 5.00,  1.00,  0.00), \
( 5.00,  1.00,  2.00), \
( 5.00,  2.00,  2.00), \
( 5.00,  0.00,  3.00), \
( 5.00, -2.00,  2.00), \
( 5.00, -1.00,  2.00), \
])

img_pnts = np.array(\
[\
(286.87457713, 294.12609945), \
(353.12542287, 294.12609945), \
(361.77407152, 380.61258594), \
(403.54814303, 380.61258594), \
(320.        , 443.33402461), \
(236.45185697, 380.61258594), \
(278.22592848, 380.61258594), \
])

# Camera Intrinsic Paramter
dist_coeffs = np.zeros((5, 1))
width = 640
height = 480
focal_length = 160
center = (width / 2, height / 2)

camera_matrix = np.array(
						[[focal_length, 0, center[0]], 
						[0, focal_length, center[1]],
						[0, 0, 1]], dtype = "double"
						)


if __name__ == "__main__":
  print("Solve Perspective N Point Problem")

  print("\nCamera Matrix : ")
  print(camera_matrix)

  print("\nDistortion Coefficient")
  print(dist_coeffs)
  
  (success, rot_vec, trans_vec) = cv2.solvePnP(world, img_pnts, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)
  
  print("\nTranslation Vector")
  print(trans_vec)
  
  print("\nRotation Vector")
  print(rot_vec)
  
  print("\nRotation Matrix")
  R, jacob = cv2.Rodrigues(rot_vec)
  print(R)
  

上記スクリプトを実行すると,下記の結果(画像座標での7点の投影位置)が得られます.もともとのカメラ位置が求まりました.

Translation Vector
[[  1.17886601e-09]
 [ -8.66025415e-01]
 [  5.00000013e-01]]

Rotation Vector
[[-1.58351454]
 [-1.58351454]
 [-0.91424254]]

Rotation Matrix
[[ -2.10831463e-10   1.00000000e+00  -2.76685896e-11]
 [  5.00000003e-01   1.29377731e-10   8.66025402e-01]
 [  8.66025402e-01   1.68751291e-10  -5.00000003e-01]]

次のエントリでは,自作のアルゴリズムでPNP問題を解いてみます.それではまた!

カメラの位置・姿勢推定0 エピポーラ幾何

3D空間上の点と投影点の対応,および透視投影行列の導入まで終わりました.ここでは,ステレオカメラやカメラの移動推定に必須のエピポーラ幾何の説明をします.

異なる位置から撮影した写真の変化

下記,首里城を異なる視点から取った写真ですが,下記の写真をパッとみるだけで,人間には「一枚目の写真を撮ったカメラ」と「二枚目の写真を撮ったカメラ」の相対位置が何となくイメージできると思います.つまり,視点が異なっていても同じ要素が写真の中に移っていれば,ある程度カメラの移動を推定できるということになります.
f:id:rkoichi2001:20180125070437j:plain

異なる位置から同一物体を撮影した場合に成り立つ幾何学的関係

なんだかとっても仰々しいタイトルになってしまいましたが,「視点を変えても同じものが写っていた時には満たされるべき条件があって,それが何か?」をここでは説明していきます.ちなみに,フライングして説明してしまうと,

「視点を変えても同じものが写っていた時には満たされるべき条件・数式があって,それが何か?」を表すのがエピポーラ幾何です.

下記が,「同一点Pを位置を変えた二つのカメラで撮影した」時のエピポーラ幾何のざっくりイメージです.
f:id:rkoichi2001:20180125074055j:plain

ここで,カメラ位置Aから撮影した点Pは投影点Aとしてスクリーンに写っていますが,点Pの位置がはっきりわかっていないと仮定すると,投影点Aに対応する点は直線CP上のどこかに存在します.逆に言うと,直線CP上の点はすべて点Aに投影されます.
f:id:rkoichi2001:20180125074817j:plain

上記の投影点Aの位置情報を知っていることによって,カメラ位置BでPを撮影した時の投影点Bの位置に何か情報を与えるでしょうか?答えはYESです.下記ノートを見ていただければわかるように,投影点Aの位置がわかれば,点PのスクリーンB上の投影点は緑色線(エピポーラ線)上のどこかに投影されることがわかります.ここで,三角形PCC´をエピポーラ平面といいます.
f:id:rkoichi2001:20180125080155j:plain

エピポーラ幾何が満たす数式

スクリーンA上の投影点Aを知ることで,投影点Bの位置がエピポーラ線上のどこかに来ることを説明しました.ここからは,この拘束条件を数式で表現したいと思います.下記のノートを見ていただければわかるように,カメラ位置C,カメラ位置C’,撮影点Pで定義される三角形CC’Pは一つの平面を構成します.よって,カメラ位置Bからカメラ位置Aへの移動ベクトルtと,カメラ位置Bの座標系から見たカメラ位置Aの視線方向ベクトルの外積を計算すると,三角形CC'Pに垂直なベクトルができます.
f:id:rkoichi2001:20180126060829j:plain

これを数式で表現すると,下記のようになります.
f:id:rkoichi2001:20180126062934j:plain


上記ノートで外積を歪対称行列で表現しましたが,ベクトル演算だと扱いにくいので,行列に置きなおしただけです.上記数式が,場所の異なるカメラから同一物体を撮影した時に満たされるべき条件です.

基本行列 (Essential Matrix) の導入

満たされるべき数式を導入しましたが,コンピュータビジョンでは上記数式のtとRの部分を一つの行列で置き換えて表現します.これをE (Essential Matrix) と言います.基本行列を用いると,二つの画像における投影点の正規化画像座標 x, x' の関係を次のように簡潔に表現することができるようになります.
f:id:rkoichi2001:20180126062648j:plain

基礎行列 (Fundamental Matrix) の導入

基本行列を導入しましたが,これは「正規化画像座標 x, x' の関係」でした.では,もう一歩進んで投影点の画像座標系ではどうでしょうか?正規化画像座標と画像座標の関係は下記のように以前のエントリでふれたように内部パラメータ行列Aを用いて下記のように表されます.
f:id:rkoichi2001:20180126063513j:plain

上記の関係を「エピポーラ幾何が満たす数式」に代入すると,下記のようになります.
f:id:rkoichi2001:20180126064304j:plain

上記ノートで導入された行列Fのことを基礎行列(Fundamental Matrix)といいます.
基礎行列,基本行列を使って表現された二つの式が導かれましたが,この数式は二つの画像上の投影点の満たすべき拘束条件を表しており,これを エピポーラ拘束といいます.

カメラの位置・姿勢推定0 同次座標・斉次座標の導入

前回のエントリ「カメラの位置・姿勢推定0 透視投影モデルと座標系の定義」で若干フライングしてしまいましたが,このエントリで斉次座標(Homogeneous Coordinate)を導入します.前回のエントリから,カメラ座標系から見た三次元の点(X, Y, Z)を正規化画像座標系に投影すると下記のように表されることがわかりました.

f:id:rkoichi2001:20180123053950j:plain

上式を見ればわかるように,3Dから2Dに変換する計算をするときに簡単な行列の連鎖式で表現できません.任意の世界座標(Xw, Yw, Zw)の投影点をもとめる式も計算しましたが,計算式が複雑になってくるとこれは面倒極まりないので,投影点を3Dの座標とみなします.3Dの座標とみなした場合,正規化画像座標系ではスクリーンは Z = 1 の場所にあるので,投影点は (x, y, 1) と表現されます.これを表現すると

f:id:rkoichi2001:20180123054856j:plain

となります.

斉次座標系の導入

本当は2Dの投影点を(x, y, 1)と表現しましたが,これを斉次座標系表現といいます.どのテキストにも,「計算に便利だから斉次座標系を導入する.」って書かれているんですが,これだとなんだか無味乾燥なので,(x, y, 1)と書かれたら,「原点から距離1の位置に置かれたスクリーンに投影されている点の座標」位に考えればいいのではないでしょうか.
(ここからちょっと自分なり解釈が入ります.間違っているかも.)
で,この斉次座標の特徴なんですが,下記の図を見ていただいてもわかるように,距離1のスクリーンに投影される3Dの点は無限にあるんですよね.要は赤の直線上に乗っている点なら,すべて同じ点に投影されるので.

f:id:rkoichi2001:20180123060523j:plain

なので,斉次座標系では3つの要素が実数倍されたものも等しいとみなします.つまり(1, 2, 3)と(2, 4, 6)と(2000, 4000, 6000)は同じものとして扱います.このように扱うことで,「3D空間では異なる場所にあるが,スクリーンに投影すると同じ場所に投影されるもの」を同じものとしてみなすことができます.これを数式的で表現すると...

f:id:rkoichi2001:20180123062121j:plain

ノートの最後に少し記述してありますが,例えばスクリーンの投影点 x を考えたときに,同じ点に投影される点の集合 λx は同値(equivalent)であるといいます.またこの λ を変えていくと λx はノートに書いている直線になりますが,これを同値類(equivalent class)といいます.

斉次座標系の3D空間への導入

2Dで導入した斉次座標を3Dにも投入します.射影幾何学的にはちゃんとした理由があるんだと思いますが,3Dの説明はいまいち思いつきません.ただ,同じノリで数式を展開するのみ!です.同じように,本来3つの要素で表現できる3D空間上を表現するために4つの要素を使います.

f:id:rkoichi2001:20180123063107j:plain

ただ,わかりやすいメリットとして,3次元空間上の点を斉次座標で表現することによって,回転移動・並進移動を行列計算で表現することがてきます.
f:id:rkoichi2001:20180123065319j:plain

剛体変換,特殊ユークリッド変換(SE(3))

ちなみに,この行列で表される移動は剛体変換 (Rigid Body Motion)といいます.なぜかというと,この行列で点群を移動させても異なる二つの点の距離が変わらないからです.別名特殊ユークリッド変換 (Special Euclidean Transformation) SE(3)とも呼ばれ,次のように定義されます.

SE(3) ≡ {g=(R, T) | R ∈ SO(3), T∈ℝ3}

このSE(3)ですが,論文を読んでるとちょこちょこ出てきて,何のことかいな.と思っていたんですが,剛体変換の群のことだったんですね!

斉次座標系を用いた3D空間上の点とスクリーン上の投影点の関係

斉次座標を2D, 3Dの点に導入したので,この結果を使って3D空間上の点とスクリーン上の投影点の関係式を表現します.まず,2D, 3Dの点それぞれの斉次座標表現は下記のようになります.

2D, 3Dの斉次座標表現

f:id:rkoichi2001:20180125053239j:plain

上記ノートの定義を用いると,カメラ座標系における点と正規化画像座標系の関係は下記のように表現できます.

カメラ座標系における投影点の表現

f:id:rkoichi2001:20180125054208j:plain

カメラ座標系から見たワールド座標系の回転要素・並進要素をそれぞれ R, t と置くと,「剛体変換」のところでまとめたように下記のように表現できます.

ワールド座標系からカメラ座標系への変換表現

f:id:rkoichi2001:20180125060049j:plain

正規画像座標から画像座標への変換は内部パラメータ行列を用いて下記のように表現できます.

正規画像座標系から画像座標系への変換表現と透視投影行列の導入

f:id:rkoichi2001:20180125060756j:plain

「カメラ座標系における投影点の表現」「ワールド座標系からカメラ座標系への変換表現」「正規画像座標系から画像座標系への変換表現」の三つの式を利用して世界座標で表現された点の画像座標への投影点の位置は下記のようにあらわされます.
f:id:rkoichi2001:20180125062058j:plain

ノートの最後で透視投影行列Pを導入しましたが,この3x4行列がわかれば,画像上のどこに点が投影されるかわかることになります.

カメラの位置・姿勢推定0 透視投影モデルと座標系の定義

Eight Point AlgorithmやFive Point Algorithm のエントリを書くにあたり,エピポーラ幾何等の内容も書かないといけないので,まず透視投影モデルとか座標系の定義を簡単にまとめておくことにしました.

透視投影モデル(Perspective Transformation Model)

コンピュータビジョン関係でカメラをモデル化する場合には,大体この「透視投影モデル」が使われています.原理的には簡単で,中学校とか小学校の理科で実験したピンホールカメラと同じようなモデルです.ただ,ピンホールカメラの場合,ピンホールより後ろに撮像面があるので,上下が反転してしまいます.これではめんどくさいので,透視投影モデルでは撮像面をピンホールの前にもってきて実像と投影像の向きが反転しないようにしています.

ピンホールカメラのイメージ

f:id:rkoichi2001:20180121215453j:plain

透視投影モデルのイメージ

f:id:rkoichi2001:20180121214814j:plain

投影で失われる情報

この書き方が正しいかどうか微妙ですが,当然ながら写真を撮影するということは3Dの世界を2Dの世界に投影するので,情報が欠落します.下記の絵に図示しますが,
「A. 小さいおっさんが近くにいる」
「B. 大きいおっさんが遠くにいる」
上記の二つは3Dの世界ではもちろんはっきりと違いがわかりますが,2Dの世界に落としてしまうと同じように見えてしまいます.人間は「おっさん」のサイズを回りの風景等の情報から無意識に推測しているので何となくサイズ感がわかりますが,おっさん以外なにも写っていない写真を見せられると判断するすべがなくなってしまいます.

f:id:rkoichi2001:20180121220953j:plain

画像からは「スケールが一意に決まらない」というこの特徴は,SFMでもSLAMでもVisualOdometryでもたびたび出てきます.

種々の座標系の定義

ここでは,以降で使用する座標系を定義します.

画像座標系

OpenCVとかカメラとかで映像を取ってそれを解析する場合には,画像の左上端や左下端を原点として,そこから (u, v) [pix] の形で画像上の位置を表現すると思います.この座標系を「画像座標系」といいます.

正規化画像座標系

上記の画像座標系をもう少し解析しやすくするために調整した座標を「正規化画像座標系」といいます.この座標系では光学中心から撮像面に下した垂線と撮像面との交点が原点になります.また,簡単のために焦点距離 f は1とします.

f:id:rkoichi2001:20180122055540j:plain

正規化画像座標と画像座標の関係

カメラ座標系で見た点(X, Y, Z)の投影点を正規化画像座標と画像座標で考えることで,正規化画像座標と画像座標の関係を導きます.
f:id:rkoichi2001:20180122061758j:plain
上記ノートの最後に出てきた A はカメラの内部パラメータといって「焦点距離」「光学中心」等々カメラとレンズに固有の値です.本当は Skew とかを考慮するともう少し変数が増えますが,大体はこの簡略化した形で十分かと思います.

ワールド座標と正規化画像座標・画像座標との関係

で,ここまで準備が整うと,次に知りたくなるのは
「ワールド座標系で(Xw, Yw, Zw)と表されている点は,画像のどこに移るのか?」
という点かと思います.これを計算します.
f:id:rkoichi2001:20180122065649j:plain

上記ノートの最後の式ですが,正規化画像座標での投影点 (x, y) が世界座標系から見た点座標 (Xw, Yw, Zw) から表現されたので,ワールド座標系と画像座標がつながりました.実際のカメラの投影点を計算するには内部パラメータ行列が必要ですので,下記のようになります.
f:id:rkoichi2001:20180122070744j:plain

ここまでで,
「ワールド座標系で(Xw, Yw, Zw)と表されている点は,画像のどこに移るのか?」
がわかりました.次は計算式がすっきりするように,斉次座標系を導入します.

カメラの位置・姿勢推定2 PNP問題

引き続きカメラの位置・姿勢推定問題です.PNP問題の解き方をまとめます.
問題分類のエントリで書いたように,下記を前提とします.

前提条件1:カメラの内部パラメータがわかっている.
前提条件2:基準となる座標系(ワールド座標系)が定義されており,撮影する物体(の各点)のワールド座標系での位置が事前に分かっている.

問題設定

解こうとしている問題のイメージ

上記の前提を図示してみると,下記のようなイメージかと思います.ここで,Xiは各点のワールド座標系での位置,xiはその各点をカメラで撮影した時の画像座標系での投影点です.
f:id:rkoichi2001:20180128143655j:plain

家の頂点五点が赤く塗られていますが,この頂点のワールド座標系での位置が分かっているとします.この時にカメラで撮った家の写真をみて,「この写真をとったカメラの位置は?」を求めるのがこの問題です.ちなみに,上記の絵に記載されている「R, t」がカメラの位置になります.

アルゴリズムのINとOUT

アルゴリズムに対するINとOUTは下記になります.
f:id:rkoichi2001:20180128144549j:plain
ちなみに,ノートに赤字で書いていますが,カメラ姿勢が自由度6(並進3と回転3)なので,3点(x と y のペア)が既知ならば,不定性は残るものの原理的にはカメラの位置が求まります.これをP3P問題といいます.P3P問題を解くアルゴリズムは Three Point Algorithm と呼ばれているようです.6点あれば線形の方程式を解く問題に帰着できてこれをP6P問題といいます.

P3P問題は非線形方程式を解く必要があり複雑なので,ここではP6P問題の解き方を説明していきます.今回の例のように6点を用いてDLT法で方程式を解いた場合,変数の消去をしなくてもいいので比較的簡単に解けます.

問題解決までの流れ

実際にカメラの位置を求めるには,大きく分けて二つのステップを経る必要があります.
1.DLT法を用いて透視投影行列を求める.
2.透視投影行列を R, t に分解する.

1.DLT法を用いて透視投影行列を求める.

方程式

解くべき方程式は,斉次座標表現を用いて下記のようにあらわされます.
f:id:rkoichi2001:20180128145825j:plain
ここで,上式は斉次座標表現を用いて表されており,「定数倍を許して等しいとみなす」のでした.しかしながら,カメラの位置はこの「定数倍」もはっきりさせないと一意に決めることができません.そのため,方程式を解くためにもう一つの変数も導入します.
f:id:rkoichi2001:20180128150636j:plain

この問題設定では,上記の方程式で既知の値と未知の値は下記のようになります.
既知の値:
「ワールド座標系での点座標(Xi, Yi, Zi, 1)」
「正規化画像座標系での投影点座標(xi, yi, zi, 1)」

未知の値:
「透視投影行列(カメラ行列)の各要素12個(pij)」
「新たに導入した定数倍ファクター λi」

透視投影行列は12個の要素を持ちますが,実際には定数倍を許して等しいとみなすので,自由度は11です.また,点が一つ増えることに定数倍ファクターのλiが一つ増えていきます.つまり,未知数と方程式の関係は下記のようになります.

既知の点ペア1つがもたらす線形方程式の個数:3
既知の点ペア1つがもたらす未知数の個数:1
透視投影行列の未知数:11

上記より,下記の条件が満たされれば線形方程式が解けます.
3N ≧ 11 + N
これを解くと,N ≧ 6となって,結果的に6点の対応関係がわかればカメラの位置が求まることになります.

DLT法を適用するための方程式展開

下記,方程式を展開して,DLT法が適用できるように Mx=b の形にします.下記のノートを見ていただければわかるように,この方程式では右辺が 0 になります.このような方程式を同次連立一次方程式(Homogeneous Linear Equations)といいます.
f:id:rkoichi2001:20180128160132j:plain

上式が1つの点が与える方程式です.この方程式が6つあれば,問題が解けることになります.N点が与えられた時の上記方程式を表すと,ベクトル表現と用いて下記のように表現できます.
f:id:rkoichi2001:20180128161336j:plain
だんだん数式が複雑になってきました....ここで,上記方程式の v を求めれば,透視投影行列が求まることになります.この数式は v = 0 が一つの解となることは自明ですが,明らかにこの解には興味がないので, v = 0 以外で上記の方程式を探します.つまり,v = 0 以外で,Mv が最小となる v を求めればよいことになります.また,透視投影行列には定数倍の不定性があり,要素は12個あるけれども,実際の自由度は11でした.このことより ||v|| = 1 という拘束条件をつけ足してあげると,この問題は下記のように表現できます.

f:id:rkoichi2001:20180128163130j:plain

上記の最小化問題を特異値分解を使って解きます.
f:id:rkoichi2001:20180128164159j:plain

係数行列 M を直行行列 U, V と対角行列 S を用いて表せました.ここで,問題は Mv = 0 (=|| Mv || がもっとも小さくなる v) となるような v を見つけることでしたので,特異値分解した行列 S から最小固有値を探せば,それに対応する固有ベクトルが v となります.
(このコラムでは特異値分解はちょっと触れられませんが,また別の機会にエントリを書きます.)

ふー.ここまでの計算で,透視投影がやっと求まりました.ただ,ここから R, t を求めなければ行けないので,もう一仕事です.

2.透視投影行列を R, t に分解する.

以前のエントリで説明しましたたが,透視投影行列と内部パラメータ行列,外部パラメータ行列の関係は下記のように表現されます.
f:id:rkoichi2001:20180128172820j:plain

ここまでの手順で,透視投影行列 P は求まっているので,P を分解して R, t の行列・ベクトルを求めることができれば,カメラの位置が分かったことになります.ここで,内部パラメータ行列 K が上三角行列であること,Rが回転行列であることを利用して RQ分解を使って P を分解します.
f:id:rkoichi2001:20180128174645j:plain

ここで,行列 A は透視投影行列 P を構成する 3x3 の行列であり,1のステップで既知です.なので,上記ノートの最後の行列式は R3 から順番に解いていくことができます.
f:id:rkoichi2001:20180128175616j:plain

次にR2を解きます.
f:id:rkoichi2001:20180128180358j:plain

最後にR1を解きます.
f:id:rkoichi2001:20180128181515j:plain

ここまでで,回転行列 R が求まりました.ここで,内部パラメータ行列 K の要素 (3, 3) は定義からは 1 となるはずですが,この手順のどこでもその条件を入れていないので, K の要素すべてを (3, 3) 要素で割ってあげれば内部パラメータ行列になります.t に関しては,内部パラメータ行列 K と並進ベクトル t からなる線形の方程式を解いてあげれば求まるかと思います.ということで,PNP問題の DLT 法を用いた解法でした.

カメラの位置・姿勢推定1 問題分類

三次元復元をすべく,カメラ・幾何学周りを勉強してますが,どうもやっぱりピンとこず...ひとまず,理解したことをまとめておくことにしました.

0.はじめに

「撮影した画像からカメラの位置・姿勢を推定する」というという問題を考えます.ここで,事前に分かっていることによって複数のケースに分かれます.

1.一枚の写真からカメラの位置を求める.

必要条件1:事前に撮影している物体(対応点)のワールド座標がわかっている.
必要条件2:カメラの内部パラメータがわかっている.

問題設定としては,下記のようになるかと思います.
f:id:rkoichi2001:20180121122648j:plain

上図の状況としては,まずカメラで撮影した家の各点(赤の点)のワールド座標位置が事前に分かっています.で,この「事前に位置の分かっている点」を「位置の分からないカメラ」で撮影したときに,「カメラの位置」を求める.という問題になります.この問題はPNP問題 (Perspective N Point Problem) と呼ばれます.

ー>PNP問題

2.複数(ここでは二枚の画像)の画像からカメラの位置を求める.

必要条件1:複数の画像で,それぞれの対応点がわかっている.(例:一枚目の写真で写っている”鼻”の位置が,二枚目の写真のどこに移っているかわかっている.)
必要条件2:カメラの内部パラメータがわかっている.

問題設定としては,下記のようになるかと思います.
f:id:rkoichi2001:20180121124215j:plain

上図の状況としては,家の写真を1の場所で撮影し,次に家の写真を2の場所で撮影しています.二枚の写真に写った家の各店(赤の点)の対応関係がわかっているときに,「1から2へのカメラの姿勢変化・移動量」を求めます.なお,後述する理由から,この問題設定では「回転」「移動方向」までは決まるものの,実際に移動した距離までは求まりません.この問題は 5点問題 (Five Point Problem) ,8点問題 (Eight Point Problem) と呼ばれます.

ー>5点問題,8点問題

次のエントリで一つずつ追いかけていきます.