OpenCVのundistort()のアルゴリズムを理解する＆逆にdistort()を実装してみる

OpenCVの歪み補正関数`cv2.undistort()`(python版)/`cv::undistort()`(C++版)の使い方の解説記事は星の数ほどありますし、皆さんも日々アンディストートしていると思います。

今回はOpenCVの公式ドキュメントや本体のコードを参照しつつ、内部で実装されているアルゴリズムについて解説したいと思います。
そして逆の機能、画像を歪ませる関数`distort()`を作ってみたいと思います。


## TL;DR
- 点を歪ませる式はclosed-formで書ける
- 点の歪みを補正する式は反復法で解く
- 画像の歪みを補正する **`undistort()`は点を歪ませる式を使って実装されている**
- 画像を歪ませる`distort()`は点の歪みを補正する式を使って実装できる（コードはこの記事の末尾）


## OpenCVにおける歪みのモデル化

OpenCVにおける歪みとはなんなのかを理解するために公式ドキュメントをみてみましょう（記事執筆時点では4.10.0）

https://docs.opencv.org/4.10.0/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d


歪み適用後の画像座標点$u, v \$は閉形式（closed-form）で書け、以下の式で計算できます。


```math
\begin{bmatrix}
u \\
v
\end{bmatrix} = \begin{bmatrix}
f_x x'' + c_x \\
f_y y'' + c_y
\end{bmatrix}
```


```math
\begin{bmatrix}
x'' \\
y''
\end{bmatrix} = \begin{bmatrix}
x' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + 2 p_1 x' y' + p_2(r^2 + 2 x'^2) + s_1 r^2 + s_2 r^4 \\
y' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\
\end{bmatrix}

```

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

```math
\begin{bmatrix}
x'\\
y'
\end{bmatrix} = \begin{bmatrix}
X_c/Z_c \\
Y_c/Z_c
\end{bmatrix}
```

$x' \$と$y' \$は正規化2次元座標です。
世界座標の3次元点にカメラのextrinsicをかけてカメラ座標$(X_c, Y_c, Z_c)\$にもってきて、$X_c, Y_c\$を$Z_c\$で割ることで正規化した点です。

大事なのは2行目です。仰々しい多項式が見えていますね。これが歪みの計算式です。

歪み係数の$k_{x}\$はradial distortionで、レンズが凸レンズや凹レンズといった半球面の形状をしているため発生する歪みを表現しており、イメージセンサの中心からの半径方向の距離$r\$を用いた多項式で近似しています。
$ 1 + k_1 r^2 + k_2 r^4 + k_3 r^6 \$が単調減少なら格子が樽のように歪むのでbarrel distortion、単調増加ならクッションのように歪むのでpincushion distortionと呼ばれます。
一般的な歪みのイメージはこの項ですね。

$p_1, p_2\$はtangential distortionでレンズの光軸に対してイメージセンサが垂直ではなく製造時に少し傾いて取り付けられてしまったことによる歪みを表現しています。傾きだからタンジェントということです。

（$s_1, s_2, s_3, s_4$はprism distortionというものらしいですが、`calibrateCamera()`のデフォルトでは無効になっていますし、本筋とはあまり関係なく私もよくわかってないので説明は割愛します。）


この式から明らかなように、歪んだ座標値を求める式は歪み係数を除くと`r`、`x`、`y`にしか依存していません。言い換えると三次元のデプス値`z`に依存していません。
つまり2次元の変換として実装することができます。

OpenCVのカメラモデルにおいては、3次元点はピンホールカメラモデルによって投影され2次元点になり、そして画像処理的に2次元的な処理として歪みが適用されるという流れになります。

実際のカメラにおいては絞りに半径があるのでピンホールではないですし、歪みは3次元の光線の進行方向がレンズ（複数枚かもしれません）によって曲げられて引き起こされます。
つまり歪みは3次元的な物理現象です。
しかし、レンズとイメージセンサ（と、露光やフォーカス等）が固定されていれば、レンズに入ってきたある光線がどう歪んで3次元平面＝2次元のイメージセンサ上のどこにヒットするのかは決定的です。
歪みは2次元的な処理で十分表現できるのですね。（波長による色収差等は当然表現できませんが）

## 画角が広いことによる歪みは対象外

OpenCVで扱っている”歪み”とは、物理的なレンズやイメージセンサの組み合わせから製造された現実のカメラが、ピンホールカメラという理想的なモデルとマッチしない部分を吸収するための仕組みです。

歪みといえば近年のスマホに搭載されている（超）広角カメラによる歪みがよくネタになったりしていますね。
人間の目の画角より画角が広いカメラだと近くのものが大きくなりすぎて違和感ありますよね。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">息子連れて初めての海だー！<br>記念に写真撮ろ〜❣️<br><br>と思ったら広角レンズ💢<br>王騎将軍倒せそうだわ💢 <a href="https://t.co/YPpJjmnaiD">https://t.co/YPpJjmnaiD</a> <a href="https://t.co/d384oBsUyG">pic.twitter.com/d384oBsUyG</a></p>&mdash; mami 🏉 (@mm___y12) <a href="https://twitter.com/mm___y12/status/1829721447172030477?ref_src=twsrc%5Etfw">August 31, 2024</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


このような画角が広いことによる透視投影歪みはOpenCVで扱う歪みの補正の対象外です。
ピンホールカメラという理想的なカメラでも必然的に発生する歪みだからです（ピンとこない人はMeshLabやBlenderなどのCGソフトで画角を変えて遊んでみましょう）。

透視投影歪みでは直線は直線になります。
OpenCVが補正したい歪みとは直線が曲がってしまう歪みです。


## 歪みを補正する式と3次元点の歪みを補正する`undistortPoints()`の実装
さて歪んでいない点をどう歪ませるのか、について前項でみました。
次はお待ちかねの歪みを除去する式です。

さきほどの歪みの式
```math
\begin{bmatrix}
x'' \\
y''
\end{bmatrix} = \begin{bmatrix}
x' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + 2 p_1 x' y' + p_2(r^2 + 2 x'^2) + s_1 r^2 + s_2 r^4 \\
y' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\
\end{bmatrix}

```
において$x'' , y'' \$が歪んだ後で、$x' , y' \$が歪む前の2次元座標です。
$x'' , y'' \$を既知として、$x' , y' \$を求めることが歪みをとるということに相当します。

解けそうでしょうか？
変数同士の掛け算が大量にあって解析的に解くのは難しそうですね。

3次元点の歪みをとる`undistortPoints()`のドキュメントには反復法を使っているという記述があります。

```
where undistort is an approximate iterative algorithm that estimates the normalized original point coordinates out of the normalized distorted point coordinates ("normalized" means that the coordinates do not depend on the camera matrix).
```

具体的にはどんな感じなのでしょうか？
OpenCVのC++のソースコードをみてみましょう。

https://github.com/opencv/opencv/blob/71d3237a093b60a27601c20e9ee6c3e52154e8b1/modules/calib3d/src/undistort.dispatch.cpp#L491


ややゴツいのですが該当部分をそのまま抜き出しました。
```C++
for( int j = 0; ; j++ )
{
    if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
        break;
    if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
        break;
    double r2 = x*x + y*y;
    double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
    if (icdist < 0)  // test: undistortPoints.regression_14583
    {
        x = (u - cx)*ifx;
        y = (v - cy)*ify;
        break;
    }
    double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
    double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
    x = (x0 - deltaX)*icdist;
    y = (y0 - deltaY)*icdist;

    if(criteria.type & cv::TermCriteria::EPS)
    {
        double r4, r6, a1, a2, a3, cdist, icdist2;
        double xd, yd, xd0, yd0;
        cv::Vec3d vecTilt;

        r2 = x*x + y*y;
        r4 = r2*r2;
        r6 = r4*r2;
        a1 = 2*x*y;
        a2 = r2 + 2*x*x;
        a3 = r2 + 2*y*y;
        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;

        vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
        xd = invProj * vecTilt(0);
        yd = invProj * vecTilt(1);

        double x_proj = xd*fx + cx;
        double y_proj = yd*fy + cy;

        error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
    }
}
}
```

なんだかループしてますね。反復法であることはわかります。

しかしコメントもなく手法の詳細は一見わかりません。
コードを見る限り歪ませる式がそのまま書いてあるように見えます。

```C++
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
```
が変数をイテラティブに更新している処理の本体です。

```math
\begin{bmatrix}
x_{n+1}' \\
y_{n+1}'
\end{bmatrix} \leftarrow \begin{bmatrix}
(x'' - (2 p_1 x'_{n} y'_{n} + p_2(r^2_{n} + 2 x'^2_{n}) + s_1 r^2_{n} + s_2 r^4_{n})) \frac{1 + k_4 r^2_{n} + k_5 r^4_{n} + k_6 r^6_{n}}{1 + k_1 r^2_{n} + k_2 r^4_{n} + k_3 r^6_{n}} \\
(y'' - (2 p_1 x'_{n} y'_{n} + p_2(r^2_{n} + 2 y'^2_{n}) + s_1 r^2_{n} + s_2 r^4_{n})) \frac{1 + k_4 r^2_{n} + k_5 r^4_{n} + k_6 r^6_{n}}{1 + k_1 r^2_{n} + k_2 r^4_{n} + k_3 r^6_{n}} \\
\end{bmatrix}
```

```math
r^2_{n} = x^2_{n} + y^2_{n} 
```

歪みをかける式を$x'=...\$の形に変形し、1ステップ前の値で右辺を計算して$x'\$を更新しています。

```C++
if(criteria.type & cv::TermCriteria::EPS)
{
    double r4, r6, a1, a2, a3, cdist, icdist2;
    double xd, yd, xd0, yd0;
    cv::Vec3d vecTilt;

    r2 = x*x + y*y;
    r4 = r2*r2;
    r6 = r4*r2;
    a1 = 2*x*y;
    a2 = r2 + 2*x*x;
    a3 = r2 + 2*y*y;
    cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
    icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
    xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
    yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;

    vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
    invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
    xd = invProj * vecTilt(0);
    yd = invProj * vecTilt(1);

    double x_proj = xd*fx + cx;
    double y_proj = yd*fy + cy;

    error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
}
```
このブロックでは停止条件の判定に使うロスを計算しています。
歪みを取った後の値に再度歪みをかけさらにピクセル座標に変換し、元の歪んだピクセル座標でユークリッド距離を計算しています。


このアルゴリズムについてインターネット上に一次情報は見当たらなかったのですが、どうやら非線形ヤコビ法のようです。
変数の値を順番に更新していくだけの単純なアルゴリズムです。

https://stackoverflow.com/questions/54103687/what-numerical-iterative-algorithm-is-actually-used-in-cvundistortpoints

もう少しソースコードを読んでみます。

初期値`x, y`は元の歪んだ正規化2次元座標のようです。

https://github.com/opencv/opencv/blob/71d3237a093b60a27601c20e9ee6c3e52154e8b1/modules/calib3d/src/undistort.dispatch.cpp#L485

```C++
x0 = x = invProj * vecUntilt(0);
y0 = y = invProj * vecUntilt(1);
```

デフォルトで最大イテレーション数は5、最小ロスは0.01に設定されているようです。

https://github.com/opencv/opencv/blob/71d3237a093b60a27601c20e9ee6c3e52154e8b1/modules/calib3d/src/undistort.dispatch.cpp#L563

```C++
cvUndistortPointsInternal(_src, _dst, _cameraMatrix, _distCoeffs, matR, matP,
                          cv::TermCriteria(cv::TermCriteria::COUNT, 5, 0.01));
```

こんな単純な実装で実用的な精度が出て世界中の人が使っているというのはなんだか不思議な気がしますが……
解析的に解くのは難しいとはいえ、1点あたり2変数ですし、高々`r`について12次、`x',y'`について13次の多項式なので、そこまで気合をいれた反復法を使う必要もないのでしょう。

このように歪みの除去も歪ませる処理と同様に2次元の処理として実装することができます（デプス画像などなくてもRGB画像だけで`undistort()`使えますよね）。


## 2次元点の歪みを補正する`undistortImagePoints()`
ここまでは3次元点を対象にしてきました。
しかし、歪みの補正は2次元画像に対して行いますよね？
2次元点はどう扱えばいいのでしょうか？

歪みの適用と補正は正規化2次元座標$(x', y') \$で行われるのでした。
画像中の2次元点からこれを計算してみましょう。

画像中の2次元点を3次元同次座標系における点として解釈し$(u', v', 1)\$と置くと、ピンホールカメラの逆投影に基づいて
```math
\begin{bmatrix}
X_c \\
Y_c \\
Z_c 
\end{bmatrix}
= \begin{bmatrix}
(u' - c_x) / f_x \\
(v' - c_y) / f_y \\
1
\end{bmatrix}
```
なので
```math
\begin{bmatrix}
x'\\
y'
\end{bmatrix}
= \begin{bmatrix}
X_c/Z_c \\
Y_c/Z_c
\end{bmatrix}
= \begin{bmatrix}
(u' - c_x) / f_x \\
(v' - c_y) / f_y
\end{bmatrix}
```
となります。
以降の計算は3次元版と全く同じです。

以上の変換をラップしているのが`undistortImagePoints()`です。

## `undistort()`の実装
これまでは点をどう歪ませ補正するのかをみてきました。

さてお待ちかね、画像の歪み補正を行う関数`undistort()`の解説をしていきましょう。
ドキュメントを参照すると
```
The function is simply a combination of initUndistortRectifyMap (with unity R ) and remap (with bilinear interpolation). See the former function for details of the transformation being performed.
```

`undistort()`は`initUndistortRectifyMap()`と`remap()`を組み合わせだけのものである、と書いてあります。

では次に`initUndistortRectifyMap()`のドキュメントを見ると

```math
\begin{array}{l}
x  \leftarrow (u - {c'}_x)/{f'}_x  \\
y  \leftarrow (v - {c'}_y)/{f'}_y  \\
{[X\,Y\,W]} ^T  \leftarrow R^{-1}*[x \, y \, 1]^T  \\
x'  \leftarrow X/W  \\
y'  \leftarrow Y/W  \\
r^2  \leftarrow x'^2 + y'^2 \\
x''  \leftarrow x' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}
+ 2p_1 x' y' + p_2(r^2 + 2 x'^2)  + s_1 r^2 + s_2 r^4\\
y''  \leftarrow y' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}
+ p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\
s\begin{bmatrix} x'''\\ y'''\\ 1 \end{bmatrix} =
\begin{bmatrix} R_{33}(\tau_x, \tau_y) & 0 & -R_{13}(\tau_x, \tau_y) \\
                0 & R_{33}(\tau_x, \tau_y) & -R_{23}(\tau_x, \tau_y) \\
                0 & 0 & 1
\end{bmatrix} R(\tau_x, \tau_y) \begin{bmatrix} x''\\ y''\\ 1 \end{bmatrix}
\\
map_x(u,v)  \leftarrow x''' f_x + c_x  \\
map_y(u,v)  \leftarrow y''' f_y + c_y
\end{array}
```
というどこかで見た式が書いてあります。
そう、これは歪ませる式です！
歪みをとる式ではありません。

最初見たときは意味がわかりませんでした。
なぜ歪ませる式がここに出てくるのか？

~~ピクセル毎に`undistortImagePoints()`を呼ぶのではないのか？~~
~~ここに書いてあるのは`undistortPoints()`のコードに書いてあった非線形ヤコビ法の概要なのか？~~
以上はどちらも間違いです。

実は、点の歪みの除去と比較して画像の歪みの除去には制約があります。
ご存知の通り、ラスター画像は整数値のピクセル位置にあるピクセルの集合です。
画像の場合は歪み除去後の座標が整数のピクセル座標である必要があるという制約がでてきます。
また、歪み除去後の座標値に対する歪んだ後の座標値は与えられていません。

歪みをとることを考えているので、点においては素直に歪みあり→歪みなしという方向の変換を考えてきました。
しかし、画像において歪みなしの座標が整数値であることを保証するには、逆に考えるのがスマートなのです。

:::note info
OpenCVの画像の歪み補正関数`undistort()`のアルゴリズムの概要は以下の通りです。

- `initUndistortRectifyMap()`
  - **歪み除去後の整数値の座標を歪ませ、対応する歪んだ座標値（小数値）を得る**
- `remap()`
  - **歪んだ画像から歪んだ座標値を使って色を取得（補間）し、歪み除去後の座標にコピーする**
:::


最初は意外に感じるかもしれませんが、この方針で上手くことは少し考えれば自明です。


## `undistort()`の別の実装を考えてみる

点の歪みをとる関数が既にあるので、歪んでいる画像の各ピクセルに対してその関数を適用すればいいのでは？というのがナイーブな発想です。

しかし、歪んでいる画像の各ピクセルに対して`undistortImagePoint()`をかけたとして変換先の座標値が整数である保証はありません。
以下のような様々な問題が発生します。

- 歪みあり画像の複数ピクセル（小数値）が歪みなし画像の1ピクセル（整数値）に対応する可能性がある
  - 平均化や補間をどうするか？
- 歪みなし画像のあるピクセルに対し歪みあり画像が1ピクセルも対応せず、欠けが発生する可能性がある
  - 元の歪みあり画像の解像度を上げたとしても解決する保証はない
  - 周囲から補間するにしてもやり方は？
    - 歪みが非線形なので標準的な線形補間を使うのは正しくない
- 点の歪み除去処理は反復法で解いており、精度面速度面で歪み付与より劣っている
  - これについてはそこまで大きな問題ではないかも 

というわけで現状のアルゴリズムが全ての面で優れていると思います。


## 点を歪ませる関数`distortImagePoints()`と画像を歪ませる関数`distort()`のアルゴリズムを考える

ここまでで、3/2次元点の歪みをとる`undistortPoints()`/`undistortImagePoints()`、
画像の歪みをとる`undistort()`についてみてきました。

では逆に歪みをかける関数について考えてみましょう。
実はOpenCVにはそのものの関数は用意されていないようです。

歪みをかけることについてはclosed-formで書けるためただ数式をコードで写経するだけであり、スクラッチから実装することも難しくはありません。
しかし、使えるものがあれば使いたいですよね。
`projectPoints()`という点の投影を行う関数のドキュメントをみると

```
By setting rvec = tvec = [0, 0, 0], or by setting cameraMatrix to a 3x3 identity matrix, or by passing zero distortion coefficients, one can get various useful partial cases of the function. This means, one can compute the distorted coordinates for a sparse set of points or apply a perspective transformation (and also compute the derivatives) in the ideal zero-distortion setup.
```

`rvec`と`tvec`に`[0, 0, 0]`を渡せば点を歪ませる関数`distortPoints()`を実装できそうです。
`distortImagePoints()`についても実現はできそうです。


次に画像を歪ませる`distort()`について考えてみましょう。
`undistort()`と同じアプローチがとれそうです。

- `initDistortRectifyMap()`
  - 歪みありの整数値の座標の歪みをとり、対応する歪みがない座標値（小数値）を得る
- `remap()`
  - 歪みがない画像から歪みがない座標値を使って色を取得（補間）し、歪みありの座標にコピーする

`initDistortRectifyMap()`はOpenCVにないので`initUndistortRectifyMap()`を参考に`undistortImagePoints()`を使って用意する必要があります。
反復法による誤差が気になるかもしれませんが、無視できる範囲でしょう（`remap()`のバイリニア補間による誤差のほうが大きいのでは、と個人的には思います）。

`remap()`はOpenCVの関数をそのまま使えます。

この記事の末尾に上記の方針に基づいたpythonによる実装を載せています。

## よくある誤解：`undistort()`に符号を反転させたdistCoeffsを渡せは`distort()`できる?
`undistort()`に符号を反転させた`distCoeffs`を渡せば歪みをとるの逆、歪みを適用できるのではないか？と直感的に思うかもしれません。

LLM、ChatGPT o1とClaude 3.5 Sonnet、に歪みのかけ方を聞いたらどちらもそういうコードを書いてきました。

しかし`undistortPoints()`で反復法で解いていることを知っていればこの解法で上手くいくことはないだろうということは想像できます。


## `distortImagePoints()`と`distort()`のpython実装

最後にpythonでの実装を紹介します。
一部不完全ですが困ることはあまりないと思われます。

検証用のテストコードもつけています。

```python
import cv2
import numpy as np


def distortPoints(src, cameraMatrix, distCoeffs):
    image_points, jac = cv2.projectPoints(
        src, np.zeros(3, src.dtype), np.zeros(3, src.dtype), cameraMatrix, distCoeffs
    )
    return image_points


def distortImagePoints(src, cameraMatrix, distCoeffs):
    src_homo = np.concatenate(
        [src, np.ones_like(src[..., 0:1])], dtype=src.dtype, axis=-1
    )

    # Bring src points into normalized cooridnate
    src_homo[..., 0] = (src_homo[..., 0] - cameraMatrix[0][2]) / cameraMatrix[0][0]
    src_homo[..., 1] = (src_homo[..., 1] - cameraMatrix[1][2]) / cameraMatrix[1][1]

    return distortPoints(src_homo, cameraMatrix, distCoeffs)


def initDistortRectifyMap(
    cameraMatrix, distCoeffs, R, newCameraMatrix, size, m1type, more_iter=False
):
    # TODO:
    # Use m1type
    w, h = size
    u, v = np.meshgrid(np.arange(w), np.arange(h), indexing="ij")
    src = np.stack([u, v], axis=-1).reshape(-1, 2)

    if more_iter:
        # More iteration than default
        # But improvement is subtle
        dst = cv2.undistortPointsIter(
            src.astype(np.float32),
            cameraMatrix.astype(np.float32),
            distCoeffs.astype(np.float32),
            R.astype(np.float32),
            P=newCameraMatrix.astype(np.float32),
            criteria=(cv2.TermCriteria_COUNT | cv2.TERM_CRITERIA_EPS, 100, 1e-6),
        )
    else:
        dst = cv2.undistortPoints(
            src.astype(np.float32),
            cameraMatrix.astype(np.float32),
            distCoeffs.astype(np.float32),
            R.astype(np.float32),
            P=newCameraMatrix.astype(np.float32),
        )

    dst = dst.reshape(w, h, 2)
    map1 = dst[:, :, 0].astype(np.float32).T
    map2 = dst[:, :, 1].astype(np.float32).T

    return map1, map2


def distort(
    src,
    cameraMatrix,
    distCoeffs,
    interpolation=cv2.INTER_LINEAR,
    borderMode=cv2.BORDER_CONSTANT,
    borderValue=0,
):
    map1, map2 = initDistortRectifyMap(
        cameraMatrix,
        distCoeffs,
        np.eye(3),
        cameraMatrix,
        src.shape[:2][::-1],
        cv2.CV_32FC1,
    )

    return cv2.remap(
        src,
        map1,
        map2,
        interpolation=interpolation,
        borderMode=borderMode,
        borderValue=borderValue,
    ).astype(src.dtype)


if __name__ == "__main__":
    import requests
    import os
    import time

    # Download Mona Lisa image from wikipedia
    url = "https://upload.wikimedia.org/wikipedia/commons/7/76/Leonardo_da_Vinci_-_Mona_Lisa.jpg"
    filename = os.path.basename(url)
    if not os.path.exists(filename):
        r = requests.get(url, stream=True)
        with open(filename, "wb") as f:
            for chunk in r.iter_content(chunk_size=1024):
                if chunk:
                    f.write(chunk)
                    f.flush()

    img = cv2.imread(filename)
    h, w, c = img.shape
    fx = fy = max(w, h)
    cx, cy = w / 2, h / 2
    camera_matrx = np.array(
        [
            [fx, 0, cx],
            [0, fy, cy],
            [0, 0, 1],
        ]
    )
    dist_coeffs = np.array([-0.5, 0.3, 0.02, -0.01, 0.001, 0.002, -0.001, -0.001])

    print("OpenCV version", cv2.__version__)
    print("numpy version", np.__version__)
    print()

    print("Input image size:", img.shape)
    print("Camera matrix:", camera_matrx)
    print("Distortion coefficients:", dist_coeffs)
    print()

    # Apply distortion to the image
    st = time.time()
    distorted = distort(img, camera_matrx, dist_coeffs)
    et = time.time()
    print("distort()      :", et - st, "sec.")
    cv2.imwrite("distorted.jpg", distorted)

    # Re-undistort the distorted image
    st = time.time()
    undistorted = cv2.undistort(
        distorted, camera_matrx, dist_coeffs, newCameraMatrix=camera_matrx
    )
    et = time.time()
    print("cv2.undistort():", et - st, "sec.")
    cv2.imwrite("undistorted.jpg", undistorted)

    print()

    print("Comparing the orignal and the re-undistorted images")
    diff = undistorted.astype(float) - img.astype(float)
    mse = np.mean(diff**2)
    print(f"MSE: {mse}")
    psnr = cv2.PSNR(img, undistorted)
    print(f"PSNR: {psnr}")

    def colorize(values, min_value, max_value, colormap="bwr"):
        import matplotlib.pyplot as plt

        values = values.astype(float)
        h, w = values.shape[:2]
        if len(values.shape) > 2:
            values = values[..., 0]  # Use B-channel

        # Ensure values are within the range
        values = np.clip(values, min_value, max_value)

        # Normalize the values to 0-1
        normalized = (values - min_value) / (max_value - min_value)

        # Get the colormap
        cmap = plt.get_cmap(colormap)

        # Map the normalized values to colors
        colors = cmap(normalized.ravel())
        # Return the RGB components (first three elements of the tuple) for each value
        return (colors[:, :3].reshape(h, w, 3) * 255).astype("uint8")

    diff_gray = cv2.cvtColor(undistorted, cv2.COLOR_BGR2GRAY).astype(
        float
    ) - cv2.cvtColor(img, cv2.COLOR_BGR2GRAY).astype(float)
    color = colorize(diff_gray, -10.0, 10.0)
    cv2.imwrite("diff.jpg", color)
```

私の環境での実行結果です。
```
OpenCV version 4.8.0
numpy version 1.25.2

Input image size: (3591, 2403, 3)
Camera matrix: [[3.5910e+03 0.0000e+00 1.2015e+03]
 [0.0000e+00 3.5910e+03 1.7955e+03]
 [0.0000e+00 0.0000e+00 1.0000e+00]]
Distortion coefficients: [-0.5    0.3    0.02  -0.01   0.001  0.002 -0.001 -0.001]

distort()      : 0.7589342594146729 sec.
cv2.undistort(): 0.09256553649902344 sec.

Comparing the orignal and the re-undistorted images
MSE: 4.723037615153465
PSNR: 41.3885895647509
```

|元画像 <br>|歪ませた画像:<br> distorted.jpg|歪みを補正した画像: <br> undistorted.jpg|元画像との差分: <br>diff.jpg|
|:---:|:---:|:---:|:---:|
| <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/250024/e1d5c195-0b86-41f4-8683-a46bcb4ff9e7.jpeg" width="128">|  <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/250024/513b3e96-7286-06fd-b59b-637de5d51b2c.jpeg" width="128"> |  <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/250024/889d1c4b-b616-8fd5-24e6-7352d03a33f0.jpeg" width="128"> | <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/250024/98b2b0d1-d5a6-245b-c5cd-cfecc4fa9f2e.jpeg" width="128">

自前実装した`distort()`で画像を歪ませ、`cv2.undistort()`で歪みを除去しています。
歪みを除去した画像が元の画像にほぼ戻っていることから`distort()`の実装も正しく動作していると言えるでしょう。

`distort()`が`cv2.undistort()`より遅いのは、中で使われている点の歪みをとる関数`cv2.undistortPoints()`が反復法で遅いからです。
また、歪みをかけてから歪みを補正した画像は見た目的には元画像にかなり似ていてMSEやPSNRもそこまで悪くはないですが、元画像と完全には一致しません。特にエッジ近辺の値でずれが大きいです。`distort()`にも`cv2.undistort()`にも誤差があります。
歪ませた時点で潰れて解像度が下がっている部分がある、反復法の誤差、`remap()`の補間による誤差、などの要因が考えられます。ピクセル毎の値の不一致は解像度が低い場合ほど顕著になると思われます。

## 最後に
さてここまでくるとなぜOpenCVに歪ませることに特化した関数が用意されていないかがわかったのではないでしょうか？
（点や画像を歪ませたい人はそもそも少なそうですが、）用意されている関数を組み合われば歪ませる機能が容易に実現できるからだと思われます。

なんとなく使っているOpenCVの歪みの中身についての理解が少しでも深まったのなら幸いです。
今後も歪みをとったりかけたりしていきましょう。


P.S.
最後に私の好きな曲を紹介します。

https://open.spotify.com/track/7p0KPA5vjqJIQzi1tH1VVy?si=4aaKRGf9Ss-B2iHRhZDKfA