COLMAPを使った３次元復元結果の座標変換方法

# はじめに
「だーれだ？」
「えっ!?もしかして...よしこちゃん?」
いいえ、Mas-sensyn です。
COLMAPは以前ご紹介したSfM(Structure from Motion)のフリーソフトですが、SfMにはスケールがありません。
また、今回、別の方法で表された座標系に変換したい、という要望がありました。
そこで今日は、COLMAPの３次元復元結果を、別の座標系に座標変換する方法についてご紹介いたします。


# 目的
COLMAPで作られた3次元復元結果を、撮影したカメラ位置を定義している世界座標系に、座標変換する。

### 注釈
1. COLMAPの復元結果にはスケールがありません。しかし、本手法を適用することによって、復元結果にスケールも与えることができます。
1. では、もともとのCOLMAPでの３次元復元結果の座標系は何かと言うと、point cloudの中心位置に原点を定義され、スケールは、最長の方向を[-10,10]mにスケールしています。(ref: [google group](https://groups.google.com/g/colmap/c/O-8FjZnMgN0/m/1vAnCtNPBgAJ))
1. 本記事でご紹介する方法は、COLMAP自身が提供する機能[Geo-registration](https://colmap.github.io/faq.html#geo-registration)を用いる手法です。
RANSACを使用して座標変換とスケール(scale+Euclid変換)を導きます。詳細なアルゴリズムは、こちらの[ソースコード](https://github.com/colmap/colmap)を参照してください。
1. 点群を直接表している座標系には座標変換できません。即ち、point cloudを、直接、point cloudを別の世界座標で表現した世界座標に変換することはできません。
それをしたい場合は、独自にICP等を適用する必要があります。

# 結果画像の例
やりたいことをイメージしやすいように、結果画像の一例を貼っておきます。
下図のように、座標系が変換されます。この際、画像からは分かりませんが、実際にはスケールも補正されています。
![single_camera optionあり](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/753103/e453b4a2-f308-2bf1-9b69-3300f6ea2c4b.png)
座標変換前のsparse復元結果

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/753103/1678d8aa-2189-6aba-23c8-328c3d8a053e.png)
座標変換後のsparse復元結果

# 前提
###今回用いた環境
GPU: Nvidia GeForce GTX1060 (denseな結果を得るにはGPUが必須です。逆にsparseな結果だけでいいなら要りません。)
OS: Ubuntu18.04 or Ubuntu20.04

###使用したソフトウェア
COLMAP 3.7

###手順概要
はじめにsparseなreconstructionを通常の方法で行い、sparseな結果(point cloud)をCOLMAPのGeo-registrationを用いて座標変換し、その結果をdense reconstructionします。はじめのsparseなreconstructionは通常の方法なので省略しますが、それも含めた方法を知りたい場合は、最後に載せたscript例をご参照下さい。


# sparseな結果の座標変換方法
###  概要
sparseな結果(point cloud)をCOLMAPのGeo-registrationを用いて座標変換します。

###入力
画像群
最低３つ以上のカメラ位置、向きを、目的の世界座標系で表した、以下の形式のテキストファイル。ref: [COLMAP Geo-registration](https://colmap.github.io/faq.html#geo-registration)

```
image_name1.jpg X1 Y1 Z1
image_name2.jpg X2 Y2 Z2
image_name3.jpg X3 Y3 Z3
...
```

### 出力
出力ディレクトリ内に保存された、以下3つのファイル(座標系が変換されただけで、入力と全く同じ形式)
cameras.bin  images.bin  points3D.bin

### 手順
以下のreferenceそのままですが、

```
colmap model_aligner \
    --input_path /path/to/model \
    --output_path /path/to/geo-registered-model \
    --ref_images_path /path/to/text-file
```
#### reference
[COLMAP geo registration](https://colmap.github.io/faq.html#geo-registration)


# denseな結果の座標変換方法
### 概要
前節のsparseな結果を用いて、denseな結果を作成するだけです。

### 入力
sparseなgeoregistration出力ディレクトリ内に保存された、以下3つのファイル
cameras.bin  images.bin  points3D.bin

### 出力
座標変換された以下２つのファイル
meshed-poisson.ply, meshed-delaunay.ply

### 手順
全てを一つのコマンドにしたautomatic_reconstructionが使えないので、以下のreference通りにcommand lineで実行します。
reference:　[COLMAP command line interface](https://colmap.github.io/cli.html)


# 入力画像群からdenseな座標変換結果を出すまでのscript例
```
#!/bin/sh
# This is a script for COLMAP georegistration and then run dense reconstruction
####

# treat arguments
USAGE="$0 <work directory which have ../images/>"

if [ $# -lt 1 ]; then
    echo $USAGE
    exit 1
fi

# sparse reconstruction
# The project directory must contain a directory "images" with all the images.
DATASET_PATH=$1 #work directory. ../images is the image directory.
cd ${DATASET_PATH}

colmap feature_extractor \
   --database_path $DATASET_PATH/database.db \
   --image_path $DATASET_PATH/../images
   --ImageReader.single_camera 1

colmap exhaustive_matcher \
   --database_path $DATASET_PATH/database.db

mkdir $DATASET_PATH/sparse

colmap mapper \
    --database_path $DATASET_PATH/database.db \
    --image_path $DATASET_PATH/../images \
    --output_path $DATASET_PATH/sparse

# georegistrate sparse point cloud
GEOREGIDIR=georegistration
mkdir -p sparse/${GEOREGIDIR}
colmap model_aligner --input_path sparse/0 --output_path sparse/${GEOREGIDIR} --ref_images_path ../campose.txt --robust_alignment 1 --robust_alignment_max_error 0.01

# dense reconstruciton for georegistered data
mkdir $DATASET_PATH/dense

colmap image_undistorter \
    --image_path $DATASET_PATH/../images \
    --input_path $DATASET_PATH/sparse/${GEOREGIDIR} \
    --output_path $DATASET_PATH/dense/${GEOREGIDIR} \
    --output_type COLMAP \
    --max_image_size 2000

colmap patch_match_stereo \
    --workspace_path $DATASET_PATH/dense/${GEOREGIDIR} \
    --workspace_format COLMAP \
    --PatchMatchStereo.geom_consistency true

colmap stereo_fusion \
    --workspace_path $DATASET_PATH/dense/${GEOREGIDIR} \
    --workspace_format COLMAP \
    --input_type geometric \
    --output_path $DATASET_PATH/dense/${GEOREGIDIR}/fused.ply

colmap poisson_mesher \
    --input_path $DATASET_PATH/dense/${GEOREGIDIR}/fused.ply \
    --output_path $DATASET_PATH/dense/${GEOREGIDIR}/meshed-poisson.ply

colmap delaunay_mesher \
    --input_path $DATASET_PATH/dense/${GEOREGIDIR} \
    --output_path $DATASET_PATH/dense/${GEOREGIDIR}/meshed-delaunay.ply
```
###reference
[COLMAP command line interface](https://colmap.github.io/cli.html)

### 注
上記では１つのカメラを使用することを前提としています。
その場合、上記"feature_extractor"のところにあるように、"--ImageReader.single_camera 1"としておくことにより、カメラ位置復元のエラーをかなり削減できます。
下記は、single_camera optionあり、なしの比較一例です。実際のカメラ経路が不明なので、正解が分からないでしょうが、single_camera optionあり、の方が現実に近かったです。

![single_camera optionなし](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/753103/e7c069c6-0bac-82d2-8009-1cc5c28293a5.png)
single_camera optionなし

![single_camera optionあり](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/753103/e453b4a2-f308-2bf1-9b69-3300f6ea2c4b.png)
single_camera optionあり(--ImageReader.single_camera 1)


# まとめ
以上、最低３つ以上のカメラ位置、向きを、目的の世界座標系で表したテキストファイルを入力として、
COLMAPで作られた3次元復元結果を、撮影したカメラ位置を定義している世界座標系に、座標変換する方法についてまとめました。

# おわりに
COLMAPは結構メジャーなソフトなので、こういう機能も実装されていて便利だな、と感じました。
ただ、カメラ位置を定義した世界座標系を用いて座標変換を行うため、カメラ位置復元の誤差が乗るため、直接の目的が復元結果を合わせることであれば、誤差は大きくなってしまうため、ICP等で直接point cloudを合わせ込みにいったほうがいいでしょう。
それでは皆様、よいCOLMAPライフを！Ciao!