レンズ歪み変換 Distortion Parameter⇔Distortion Parameter　及び と Distortion Paramete⇔Undistortion Parameter の変換

中心投影カメラモデルは以下の式で表されます。

```math

３次元点 (x,y,z) に対して \\
x' = x / z \\
y' = y / z \\
u = fx * x' + cx \\
v = fy* y' + cy

```

ここで、$(u,v)$ は画像上の座標、$fx,fy$ は焦点距離、$cx,cy$ は投影中心という設定です。

普通は、カメラはレンズ歪みを起こすので、これも数式で表現されます。
色々なレンズ歪みの表現方法がありますが、今回は簡単のために以下のレンズ歪みを考えます。

```math
r^2 = x'^2 + y'^2 \\

x'' = x'( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{1}x'y' + p_{2}(r^2 + 2x'^2) \\

y'' = y'( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{2}x'y' + p_{1}(r^2 + 2y'^2) \\
u = fx * x'' + cx \\
v = fy* y'' + cy

```

OpenCV でよく見る表現方法ですね。
$k_1,k_2,k_3$ はは半径方向の歪み係数， $p_1 , p_2$ は円周方向の歪み係数です。

レンズ歪みパラメータは真の位置から歪ませるパラメータとなります。なので、歪みパラメータ。

ここまでが前提となります。

この歪みパラメータは数式から分かるように高次の多項式になります。
ここで、レンズ歪みの表現方法を変更したいと思った場合どうすればよいでしょうか。
例えば、OpenCV で内部キャリブレーションを行った後に、特殊なレンズ歪みに変更したいと思った場合等です。

また、歪ませるパラメータから歪みを取るパラメータに変更したい場合はどうすればよいでしょうか。

これらに対する Solution が本記事です。

## レンズ歪み間のパラメータ変換
レンズ歪みAからレンズ歪みBへの変換方法についての説明です。

考えられる状況としては、内部パラメータをキャリブした際に、焦点距離、投影中心とレンズ歪みAが推定されたが、どうしてもレンズ歪みBで歪みを表現したくなったというような状況です。
数式から分かるように、レンズ歪みと内部パラメータというものは独立しています。（これが重要）

レンズ歪みAは次式で表現されます。

```math
[Lens distortion Parameter A] \\
r^2 = x'^2 + y'^2 \\

x'' = x'( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{1}x'y' + p_{2}(r^2 + 2x'^2) \\

y'' = y'( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{2}x'y' + p_{1}(r^2 + 2y'^2) \\

```
レンズ歪みBは次式で表現されます。


```math
[Lens distortion Parameter B] \\
r^2 = x'^2 + y'^2 \\

x'' = x'( 1+k_{1}' r^2 + k_{2}' r^4 + k_{3}' r^6)  \\

y'' = y'( 1+k_{1}' r^2 + k_{2}' r^4 + k_{3}' r^6)  \\

```

まず行うこととしては、$(x',y')$ と $(x'',y'')$ の対応を与えることです。
レンズ歪みAは既に分かっているので、$z = 1$　の３次元平面上の点をランダムにサンプリングすることで、$(x',y')$ ⇔ $(x'',y'')$　を簡単に与えることができます。

レンズ歪みBは次のように表現できます。

```math
[Lens distortion Parameter B] \\
 1+k_{1}' r^2 + k_{2}' r^4 + k_{3}' r^6 = x''/x' - 1 \\
 1+k_{1}' r^2 + k_{2}' r^4 + k_{3}' r^6 = y''/y' - 1
```

これは線形なので、後は特異値分解するとかで未知パラメータ $k_{1}',k_{2}',k_{3}'$ を推定することができますね。

線形の形ではこのように簡単に求めることができます。
非線形な歪みモデルに対しても、非線形最適化で推定が可能になります。

## レンズ歪み(lens distortion) とレンズ非歪み(lens undistortion) の変換
computer vision では順投影(projection)が基本なため、レンズ歪みもそれに適した形で表現されています。
しかし、３次元計測の分野では逆投影(reprojection) を行うことも多くあります。

では、あるレンズ歪みパラメータで歪んだ画像のピクセル (u,v) を逆投影すると、どのようなRay で表現されるでしょうか。

単純に、

```math
x' = (u-cx) / fx \qquad (u = fx * x'' + cx)\\

y' = (v-cy) / fy \qquad (v = fy * y'' + cy) \\

```
ですが、これ以降の

```math

r^2 = x'^2 + y'^2 \\

x'' = x'( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{1}x'y' + p_{2}(r^2 + 2x'^2) \\

y'' = y'( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{2}x'y' + p_{1}(r^2 + 2y'^2) \\

```
これが厳しいですね。
解けないわけではないと思いますが、これを解くのは面倒くさいと思います。

では、OpenCV にこれを可能にする関数があるのでは？　と思いますが、残念ながらありません。
OpenCV では基本的には cv::undistort を使う前提に立っているのでしょうね。

ということで、先ほどと同様に新たなパラメータで lens undistortion パラメータを表現してみましょう。
これは単純に次式で表現ができます。

```math
[Lens Undistortion Parameter] \\
r^2 = x''^2 + y''^2 \\

x' = x''( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{1}x''y'' + p_{2}(r^2 + 2x''^2) \\

y' = y''( 1+k_{1} r^2 + k_{2} r^4 + k_{3} r^6) + 2p_{2}x''y'' + p_{1}(r^2 + 2y''^2) \\

```

ということでこれも線形になっているので、最小二乗法などで適当にパラメータを推定することで、目的が達成されます。

## コード

```code
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import os
import sys

'''
camera model setting

[x y z] = R[X Y Z] + t ( in general, R=I, t=0)

_x = x/z, _y = y/z

r2 = _x*_x + _y*_y

__x = _x( 1 + k1*r2 + k2*r4 + k3*r6)
__y = _y( 1 + k2*r2 + k2*r4 + k3*r6)

u = fx * __x + cx
v = fy * __y + cy

'''

reference_image_file = "referenceImage.png" # 元画像ファイル
reference_image = cv2.imread(reference_image_file,1)
height,width = reference_image.shape[:2]

# 仮想カメラパラメータの設定
fx = 1000
fy = 1000
cx = width/2 
cy = height/2
camMat = np.zeros([3,3])
camMat[0,0] = fx
camMat[1,1] = fy
camMat[0,2] = cx
camMat[1,2] = cy
camMat[2,2] = 1
print("cam Matrix : ")
print(camMat)

# 歪みパラメータの設定
k = np.zeros([5])
k[0] = 0.951532
k[1] = -1.50494
k[2] = -0.0192443
k[3] = 0.00208354
k[4] = 0.00519922
print("dist params : ")
print(k)


# 歪み画像の生成
dist_image = np.zeros([height,width,3])
for h in range(height):
	for w in range(width):
		u = float(w)
		v = float(h)

		# reprojection without distortion
		_x = (u - cx)/fx
		_y = (v - cy)/fy
		_z = 1.0
		
		# projection with distortion
		_x = _x/_z
		_y = _y/_z
		r2 = _x*_x + _y*_y
		r4 = r2*r2
		r6 = r4*r2
		_x = _x*(1 + k[0]*r2 + k[1]*r4 + k[2]*r6) + 2*k[3]*_x*_y + k[4]*(r2 + 2*_x*_x)
		_y = _y*(1 + k[0]*r2 + k[1]*r4 + k[2]*r6) + k[3]*(r2 + 2*_y*_y) + 2*k[4]*_x*_y
	
		_u = fx * _x + cx
		_v = fy * _y + cy

		#print(u,v)
		#print(_u,_v)
		
		_iu = int(_u + 0.5)
		_iv = int(_v + 0.5)
		if (_iu < 0 or _iu>=width or _iv < 0 or _iv >=height):
			continue

		dist_image[_iv,_iu] = reference_image[h,w]
cv2.imwrite("dist_image.png",dist_image)

# 非歪みパラメータの計算開始
## 対応点の計算
point_total_num = int(100*100)
point_list = np.zeros([point_total_num,4])# 0:x' 1:y' 2:x'' 3:y''
point_image = dist_image.copy()
for i in range(point_total_num):
	
	_x = (np.random.rand() - 0.5)
	_y = (np.random.rand() - 0.5)
	
	# projection with distortion
	r2 = _x*_x + _y*_y
	r4 = r2*r2
	r6 = r4*r2
	x = _x*(1 + k[0]*r2 + k[1]*r4 + k[2]*r6) + 2*k[3]*_x*_y + k[4]*(r2 + 2*_x*_x)
	y = _y*(1 + k[0]*r2 + k[1]*r4 + k[2]*r6) + k[3]*(r2 + 2*_y*_y) + 2*k[4]*_x*_y

	point_list[i,0] = _x
	point_list[i,1] = _y
	point_list[i,2] = x
	point_list[i,3] = y

print(point_list)

# 線形解法
A = np.zeros([point_total_num*2,5])
b = np.zeros([point_total_num*2])
for i in range(point_total_num):
	#_u = point_list[i,0]
	#_v = point_list[i,1]
	#__u = point_list[i,2]
	#__v = point_list[i,3]

	_x = point_list[i,0]
	_y = point_list[i,1]
	__x = point_list[i,2]
	__y = point_list[i,3]
	
	__r2 = __x*__x + __y*__y
	__r4 = __r2*__r2
	__r6 = __r4*__r2

	A[i*2,0] = __r2*__x
	A[i*2,1] = __r4*__x
	A[i*2,2] = __r6*__x
	A[i*2,3] = 2*__x*__y
	A[i*2,4] = __r2 + 2*__x*__x
	A[i*2+1,0] = __r2*__y
	A[i*2+1,1] = __r4*__y
	A[i*2+1,2] = __r6*__y
	A[i*2+1,3] = __r2 + 2*__y*__y
	A[i*2+1,4] = 2*__x*__y

	b[i*2] = _x - __x
	b[i*2+1]  = _y - __y
print(A)
print(b)

A_inv = np.linalg.pinv(A)
print(A_inv)
#x = np.linalg.solve(A,b)
x, residuals, rank, s = np.linalg.lstsq(A,b)
print(x)
print(residuals)
print(s)

# undistortion using estimated prameters
_k = x.copy()

# 非歪み画像の生成
undist_image = np.zeros([height,width,3])
for h in range(height):
	for w in range(width):
		u = float(w)
		v = float(h)

		# reprojection without distortion
		_x = (u - cx)/fx
		_y = (v - cy)/fy
		_z = 1.0
		
		# projection with distortion
		_x = _x/_z
		_y = _y/_z
		r2 = _x*_x + _y*_y
		r4 = r2*r2
		r6 = r4*r2
		_x = _x*(1 + _k[0]*r2 + _k[1]*r4 + _k[2]*r6) + 2*_k[3]*_x*_y + _k[4]*(r2 + 2*_x*_x)
		_y = _y*(1 + _k[0]*r2 + _k[1]*r4 + _k[2]*r6) + _k[3]*(r2 + 2*_y*_y) + 2*_k[4]*_x*_y
	
	
		_u = fx * _x + cx
		_v = fy * _y + cy

		#print(u,v)
		#print(_u,_v)
		
		_iu = int(_u + 0.5)
		_iv = int(_v + 0.5)
		if (_iu < 0 or _iu>=width or _iv < 0 or _iv >=height):
			continue

		undist_image[_iv,_iu] = dist_image[h,w]

cv2.imwrite("undist_image.png",undist_image)


```


## まとめ
レンズ歪みは簡単に変換できる！