ZED2i カメラの焦点距離を得る

深度情報を取得できたら、それを空間の点群PointCloud のデータに変換したい。
その際に、焦点距離 focal length が必要になる。
StereoLabs のカメラではシリアルナンバーごとに、焦点距離のデータが予め提供されている。

## 想定する読者
stereolabsのzed2, zed mini, zed x などのステレオカメラの利用者
いまどきのステレオカメラは何ができるのかを知りたい人

## 前提
ZED SDK 4.0
Ubuntu 20.04
ZED2i
Python 版のAPI

## 焦点距離 focal length の情報
StereoLabsのZEDカメラは、カメラの特性がキャリブレーションされている。
そのカメラのシリアルナンバーごとにそのファイルが管理されている。
ZED SDK のインストーラーでインストール後に、以下の場所にconfファイルが保存されている。

`/usr/local/zed/settings/SN[0-9]+.conf`

ZED SDKのカメラでは、カメラの機種ごとに設定可能な解像度が数種類ある。
それにともなって、カメラの左右・カメラの解像度モードごとに値がある。

```
head -22 SN*.conf
[LEFT_CAM_2K]
fx=1064.82
fy=1065.07
cx=1099.05
cy=628.813
k1=-0.0634518
k2=0.0401532
p1=-0.000375119
p2=0.00074809
k3=-0.0161231

[RIGHT_CAM_2K]
fx=1065.27
fy=1065.34
cx=1133.31
cy=654.957
k1=-0.0587747
k2=0.0322036
p1=5.85653e-05
p2=-0.000297978
k3=-0.0123602
```

#### Baselineの値もあるよ
上記のファイルの[STEREO] セッションにBaselineなどのその他の必要な値があります。

どうやら、tomlファイルのようだ。libtoml(python3.11以降）やtoml のライブラリでloadするとdict型で読み込めそうだ。

## zed sdk のpythonスクリプトの中からの取得例
```.py:
import pyzed.sl as sl

# Create a ZED camera object
zed = sl.Camera()

# Set up initial parameters for the camera
init_params = sl.InitParameters()

# Open the camera
status = zed.open(init_params)
if status != sl.ERROR_CODE.SUCCESS:
    print(f"Error opening camera: {status}")
    exit(1)

cam_info = zed.get_camera_information()

left_cam_params = cam_info.camera_configuration.calibration_parameters.left_cam
right_cam_params = cam_info.camera_configuration.calibration_parameters.right_cam

print("Left Camera Parameters:")
print(f"image_size: {left_cam_params.image_size.width} x {left_cam_params.image_size.height}")
print(f"Focal Length (fx, fy): {left_cam_params.fx}, {left_cam_params.fy}")
print(f"Principal Point (cx, cy): {left_cam_params.cx}, {left_cam_params.cy}")
print(f"Distortion Coefficients: {left_cam_params.disto}")

print("\nRight Camera Parameters:")
print(f"image_size: {right_cam_params.image_size.width} x {right_cam_params.image_size.height}")
print(f"Focal Length (fx, fy): {right_cam_params.fx}, {right_cam_params.fy}")
print(f"Principal Point (cx, cy): {right_cam_params.cx}, {right_cam_params.cy}")
print(f"Distortion Coefficients: {right_cam_params.disto}")

zed.close()
```

#### 実行結果の1例
```
python3 camerainfo.py 
(中略)
Left Camera Parameters:
image_size: 1280 x 720
Focal Length (fx, fy): 527.9297485351562, 527.9297485351562
Principal Point (cx, cy): 648.6941528320312, 370.4883728027344
Distortion Coefficients: [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]

Right Camera Parameters:
image_size: 1280 x 720
Focal Length (fx, fy): 527.9297485351562, 527.9297485351562
Principal Point (cx, cy): 648.6941528320312, 370.4883728027344
Distortion Coefficients: [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]
```

## 焦点距離などのカメラパラメータって何って方のために
### 「実践コンピュータビジョン」４章　カメラモデルと拡張現実感
その中で、キャリブレーション行列Kと焦点距離・カメラ中心の座標とが書かれています。

![K-matrix2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/83761/da28536c-9eec-2590-78ed-fff656540102.png)


ステレオ平行化済みの画像がえられる範囲では、この本の記述がわかりやすいです。

英語が苦にならない方ならば、次にpdfファイルがおすすめです。

Programming Computer Vision With Pyhon
http://programmingcomputervision.com/downloads/ProgrammingComputerVision_CCdraft.pdf


### 「詳解　OpenCV コンピュータビジョンライブラリを使った画像処理・認識」
この本では、カメラキャリブレーションもステレオ平行化もされていないカメラから、カメラキャリブレーションをするまでの過程について詳細がかかれている書籍です。
うれしいことには、この書籍に書かれている内容を使ってカメラキャリブレーションをするためのソフトウエアがOpenCV の実装として用意されているということです。
ただ、ZED2iを使う限りには、その必要はありません。


## 関連情報
### [【Python】深度画像から点群を作成する](https://qiita.com/ground0state/items/9591edcfbab361adc457)
```.py:
    points = []    
    for v in range(rgb.size[1]):
        for u in range(rgb.size[0]):
            color = rgb.getpixel((u,v))
            Z = depth.getpixel((u,v)) / scalingFactor
            if Z==0: continue
            X = (u - centerX) * Z / focalLength
            Y = (v - centerY) * Z / focalLength
            points.append("%f %f %f %d %d %d 0\n"%(X,Y,Z,color[0],color[1],color[2]))
```
コードの断片
このコードでは、numpyを使っていないものです。
この例ではfx, fy の区別がなく共通の値focalLengthとしています。


### qiita [深度から点群](https://qiita.com/XPT60/items/60299e2a46976a2160ee)

https://www.open3d.org/docs/0.6.0/python_api/open3d.geometry.create_point_cloud_from_rgbd_image.html

![Screenshot from 2024-08-10 22-07-11.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/83761/05496ab1-76f9-ea61-7e6e-fd0098bf8d0d.png)

の利用例になっています。

利用例のgithb https://github.com/torupati/o3d_depth2pcd

### 解説：
u, v はイメージセンサ上のpixelの位置（水平、垂直）
u, vは整数です。
z = d / depth_scaleとなっているので、
ライブラリの使い方の中で想定している長さのスケールになっています。
[mm] かもしれませんし、[m]かもしれません。
Xは水平方向の位置です。右側が＋になります。
yは鉛直方向の位置です。下側が＋になります。
上記の式から見てわかるように、焦点距離fx,fyの単位は無次元量になっています。
実際には、pixelの大きさを単位とした量になります。

ZED2i のカメラの場合には、これらのカメラパラメータが提供されているので、
すんなりと、変換作業を行うことができます。

 
### 付記：標準によせることの大事さ
depthをpointcloudに変換することができるライブラリは多数あるだろう。
その中で、できるだけ標準のライブラリを選ぼう。
標準のライブラリは、GPUへの対応もされていたり、CPUの種類のサポートもある。
標準によせることで、自分がテストしてメンテナンスしなくちゃならないコードが減る。
標準としてのライブラリ・データ形式であることで、それを利用する追加のライブラリが作りやすくなる。

[Open3D-ML](https://github.com/isl-org/Open3D-ML)

## 注意：
depth は欠損値(nan, posinf, neginfなどのisfiniteではない値)を持つことがふつうです。
欠損点が0の場合もあります。[IEEE 754](https://ja.wikipedia.org/wiki/IEEE_754)のfloatに対応していないデータ形式の場合には, depth を０として表現している場合もあります。上記のpyhtonスクリプトはその例です。
そのため、欠損値に対応したライブラリの作りになっているか、自分で確かめて見てください。

## 補足：disparity(視差) からdepth(深度）への変換
[Disparity map to point cloud using Open3d? #1744](https://github.com/isl-org/Open3D/issues/1744)
```
depth = baseline * focal_length / disparity
```
ここで、focal_lengthとdisparity の単位は[pixel] です。
depthの単位は baseline(基線長) の単位と同じになります。

# 付記：
なお、ZED SDK の場合、点群を算出するのに焦点距離を意識しなくても十分です。
[ZED SDKを使いこなす point_cloud](https://qiita.com/nonbiri15/items/3981a31efe4316dcca11)
に書いたやり方で、点群を取得できます。

にもかかわらず、焦点距離を明示的に利用する記事を書いたのは、ZED SDK 以外のライブラリを使って、視差計算・深度計算を行いたい場合に、同様に点群を算出できるようにするためです。