深度から点群

# はじめに

RGBD の情報から３Dモデルを作るところを、Open3d (python) で試してみます。
実は、下記のチュートリアル通りに行うと、あっさりできました。

http://www.open3d.org/docs/release/tutorial/geometry/rgbd_image.html

下記、メモです。

# やったこと

Open3D (python)を使って
- 深度画像と色画像から点群を生成した
- 深度画像が実際の長さに変換される過程を理解した（多分）

## 準備

### open3dの用意

これはとりあえずシェルで```python -c "import open3d"```
が怒られなくなるまで必要なものをインストールします。
自分は今はpoetry でパッケージを管理しているのですが、下記で行けました。

```
$ poetry add open3d
$ poetry add scikit-learn tqdm pandas
$ poetry add pyyaml
$ poetry add addict
$ poetry add pillow
$ poetry add matplotlib
```

### RGBD Dataset

Redwood dtaset を使います。下記でデータセットのファイルのパスを取得できます。

```Python
import open3d as o3d
print("Read Redwood dataset")
redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

print(f'Redwood Dataset {len(redwood_rgbd.color_paths)}')
rgb_filepath = redwood_rgbd.color_paths[0]
depth_filepath = redwood_rgbd.depth_paths[0]
print(rgb_filepath)
print(depth_filepath)
```

自分は特に何もしていないのですが、下記にダウンロードしてくれます。
こんな感じにデータが格納されていました。

```
$ tree /home/x77/open3d_data/extract/SampleRedwoodRGBDImages/
/home/x77/open3d_data/extract/SampleRedwoodRGBDImages/
├── camera_primesense.json
├── color
│   ├── 00000.jpg
│   ├── 00001.jpg
│   ├── 00002.jpg
│   ├── 00003.jpg
│   └── 00004.jpg
├── depth
│   ├── 00000.png
│   ├── 00001.png
│   ├── 00002.png
│   ├── 00003.png
│   └── 00004.png
├── example_tsdf_pcd.ply
├── odometry.log
├── rgbd.match
└── trajectory.log

2 directories, 15 files
```

カメラパラメータもcamera_primesense.jsonに含まれていました。 Prime Sense って何だろう。

下記を見ると、結構昔のデータセットのようです。

http://redwood-data.org/


## Redwood Dataset を使って試す

で、チュートリアルに書いてある通りに行います。下記の流れです。

1. color file (*.png), depth (*.png) を読み込みます
1. RGBD Image というクラスのインスタンスを作ります。
1. Point cloud に変換します。
1. Point cloud を保存する

### RGBの画像とDepth画像を読む

opencv ではなくopen3d の```io.read_image(...)```を使ってファイルをロードします。

```python
color_raw = o3d.io.read_image(rgb_filepath)
depth_raw = o3d.io.read_image(depth_filepath)

print(f'color: {type(color_raw)} type={color_raw.get_geometry_type()} dim={color_raw.dimension()}')
print(f'depth: {type(depth_raw)} type={depth_raw.get_geometry_type()} dim={depth_raw.dimension()}')
```

下記のように出力されました。

```shell
color: <class 'open3d.cpu.pybind.geometry.Image'> type=Type.Image dim=2
depth: <class 'open3d.cpu.pybind.geometry.Image'> type=Type.Image dim=2
```

数値データは下記のように、np.array として見れます。

```python
    print(np.asarray(color_raw).shape, np.asarray(color_raw).dtype)
    print(np.asarray(depth_raw).shape, np.asarray(depth_raw).dtype)
```
とすると、
```
(480, 640, 3) uint8
(480, 640) uint16
```
となりました。
### RGBD Image の作成

RGBとDepth、両方ともio.Image の形ですが、これを```geometry.RGBDImage.create_from_color_and_depth```に入れます。

```python
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(rgbd_image)
```

下記のように出力されました。

```shell
RGBDImage of size
Color image : 640x480, with 1 channels.
Depth image : 640x480, with 1 channels.
Use numpy.asarray to access buffer data.
```

カラー画像は0から１の間の値のグレー画像に変換されます。深度画像は単位がメートルとして格納されます。


http://www.open3d.org/docs/release/python_api/open3d.geometry.RGBDImage.html

この方法だと、カラー画像はJPEG、深度画像は16bit のPNGに限定されていますが、下記のサイトに変換して読み込む方法が書いてありました。

https://feynman.hatenablog.com/entry/2022/09/28/131208

### Point Cloud

点群は、create_from_rgbd_image を使ってできます。RGBDデータに、カメラパラメータの情報も渡します。焦点距離が分からないと、再現できないですよね。

```python
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    rgbd_image,
    o3d.camera.PinholeCameraIntrinsic(
        o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
# Flip it, otherwise the pointcloud will be upside down
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
```

この変換はチュートリアルにある通りなのですが、意味は後で考えてみます。（本当か？^^;）

ここでのカメラパラメータが気になるのですが、良く分かりません。分からないことだらけだよ。この値はどこから来るのかな。

```ipython
In [5]: p = o3d.camera.PinholeCameraIntrinsic(
   ...:         o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)

In [6]: p
Out[6]:
PinholeCameraIntrinsic with width = 640 and height = 480.
Access intrinsics with intrinsic_matrix.

In [7]: p.intrinsic_matrix
Out[7]:
array([[525. ,   0. , 319.5],
       [  0. , 525. , 239.5],
       [  0. ,   0. ,   1. ]])
In [8]: p.get_focal_length()
Out[8]: (525.0, 525.0)

In [9]: p.width
Out[9]: 640

In [10]: p.height
Out[10]: 480

In [11]: p.get_principal_point()
Out[11]: (319.5, 239.5)

In [12]: p.get_skew()
Out[12]: 0.0
```

Intrinsic Parameter は内部パラメータと呼ばれるもので、fx, fy, cx, cy　があるらしい。ここでは画素単位で扱われている。主点(principal point) 主面と光軸の交点、光学中心の方が通りが良いらしいですが、このパラメータだとcx=319.5, cy=239.5 となっていますが、width/2=320, height/2=240なので少しだけずれている、ってことなのかな？

http://www.open3d.org/docs/release/python_api/open3d.camera.PinholeCameraIntrinsic.html#open3d.camera.PinholeCameraIntrinsic


下記で保存できます。

```python
o3d.io.write_point_cloud("test.pcd", pcd)
```

結果はCloudCompare で見えました。ファイルパスに日本語が含まれているとこのソフトでは開けないようなので、注意しましょう。

## 深度の値について

### ファイルの読み方

このサンプルで利用した深度情報の扱い方についてメモしておきます。
深度情報のファイルは、uint16 で値を持っています。

```
$ file ~/open3d_data/extract/SampleRedwoodRGBDImages/depth/00002.png
/home/xtkd77/open3d_data/extract/SampleRedwoodRGBDImages/depth/00002.png: PNG image data, 640 x 480, 16-bit grayscale, non-interlaced
```

この情報について、先は Open3d を使って読みました。
```python
infile = '../../../open3d_data/extract/SampleRedwoodRGBDImages/depth/00002.png'

depth_o3d = o3d.io.read_image(infile)
print(type(depth_o3d))
print(f'dtype=', np.asarray(depth_o3d).dtype, ' shape=', np.asarray(depth_o3d).shape)
print(f'max=', np.asarray(depth_o3d).max(), ' min=', np.asarray(depth_o3d).min())
```
とすると、
```shell
<class 'open3d.cpu.pybind.geometry.Image'>
dtype= uint16  shape= (480, 640)
max= 2702  min= 0
```

OpneCVを使ってnumpy array で読んでから geometory.Image にする方法もあります。

```python
depth_cv2= cv2.imread(infile, cv2.IMREAD_ANYDEPTH )
print(type(depth_cv2))
print(f'dtype=', depth_cv2.dtype, ' shape=', depth_cv2.shape)
print(f'max=', depth_cv2.max(), ' min=', depth_cv2.min())
print('')
depth_o3d_2 = o3d.geometry.Image(depth_cv2)
print(type(depth_o3d_2))
print(f'dtype=', np.asarray(depth_o3d_2).dtype, ' shape=', np.asarray(depth_o3d_2).shape)
print(f'max=', np.asarray(depth_o3d_2).max(), ' min=', np.asarray(depth_o3d_2).min())
```

とすると、当然ながら？同じ値が入っているようです。
```shell
<class 'numpy.ndarray'>
dtype= uint16  shape= (480, 640)
max= 2702  min= 0

<class 'open3d.cpu.pybind.geometry.Image'>
dtype= uint16  shape= (480, 640)
max= 2702  min= 0
```

### 長さに変換

整数値として保持している深度の情報ですが、モデル作成では単位がメートルでなければならないはずです。これは、PointCloud.create_from_rgbd_image の解説を見てみると、下記のように depth_scale というパラメータがあります。

>Factory function to create a pointcloud from an RGB-D image and a camera. Given depth value d at (u, v) image coordinate, the corresponding 3d point is:
>
>- z = d / depth_scale
>- x = (u - cx) * z / fx
>- y = (v - cy) * z / fy

http://www.open3d.org/docs/release/python_api/open3d.geometry.PointCloud.html

depth_scale の値ですが、これは create_from_depth_image の解説のところに、

> depth_scale (float, optional, default=1000.0) – The depth is scaled by 1 / depth_scale.

とあります。なので、入力データにある2702 は2.702 mの意味ではないかと思います。

# まとめ

とりあえずやりたかった、「depthから点群」ができたので良しとします。データセットは随分と昔から作られていたんですね。そういえば、Kinect とかあったね。おじさんはぼんやり覚えています。触ったことないけれど。

明日の生き延びれるかな。


下記に自分の動作確認時のものを置いてあります。

https://github.com/torupati/o3d_depth2pcd

### 参考情報

1. Open3Dの情報もちらほらありました。大変ありがたいサイトです。
http://whitewell.sakura.ne.jp/Open3D/Open3D.html

1. Prime Sense はイスラエルの会社らしい。
https://en.wikipedia.org/wiki/PrimeSense

(2023/5/21)

[追記]
- 「深度について」の節を追加しました。2023/5/23
- PrimeSenseのリンク、自分のgithub repository の情報を追記 2023/5/23