TUM RGB-D SLAM Dataset and Benchmarkの準備・実行・評価

## 開発環境
- Windows10 64bit 
- Anaconda 2018.12
- Python 2.7.15 
- Python 3.6.5
- Open3D v0.4.0

## 準備
1．[RGB-D Dataset](https://vision.in.tum.de/data/datasets/rgbd-dataset/download)と[rgbd_benchmark_tools](https://svncvpr.in.tum.de/cvpr-ros-pkg/trunk/rgbd_benchmark/rgbd_benchmark_tools/src/rgbd_benchmark_tools/)をダウンロードしてください。
2．AnacondaでPython2.7環境を作ります。[こちら]()を参考にしてください。
3．下記コマンド（例）でrgbd_benchmark_toolsのassociate.pyを実行すると、タイムスタンプ、RGBとDepthのリストがassociations.txtに保存されます。RGBとDepthは非同期で取得されているため、タイムスタンプが近いRGBとDepthの組を生成しています。

~~~
python associate.py D:/TUM/rgbd_dataset_freiburg1_360/rgb.txt D:/TUM/rgbd_dataset_freiburg1_360/depth.txt > D:/TUM/rgbd_dataset_freiburg1_360/associations.txt
~~~

4．各シーンについてassociation.pyを実行します。

~~~python
# associate_auto.py
import glob
import subprocess
import shutil

dirnames = glob.glob("../TUM/rgbd_dataset_freiburg*/")

for dirname in dirnames:
    cmd = "python associate.py " + dirname + "rgb.txt " + dirname + "depth.txt > " + dirname + "associations.txt"
    print(cmd)
    res = subprocess.call(cmd, shell=True)
~~~

5．このような形になっていればOKです。
![50099085_2211817695524762_1611030958440448000_n.png](https://qiita-image-store.s3.amazonaws.com/0/63863/21a8c314-5636-53ee-9d21-faaeba784903.png)

# 実行
1．Open3DのRGBD Odometryを用いて、カメラ軌跡を求めるプログラムを作成しました。Python3.6.5およびOpen3D v0.4.0を用いています。Open3Dの導入方法は[こちら]()。

~~~python
# rgbd_odometry_tum.py
from open3d import *
import numpy as np
import matplotlib.pyplot as plt
import cv2
import time
import math
import sys

# python rgbd_odometry_tum.py "../../TestData/camera_tum_1.json" "D:/TUM/rgbd_dataset_freiburg1_xyz/" "HybridRGB-DOdometryResult.txt"
if __name__ == "__main__":
    print(len(sys.argv))
    if len(sys.argv) != 4:
        sys.exit()

    pinhole_camera_intrinsic = read_pinhole_camera_intrinsic(sys.argv[1])
    print(pinhole_camera_intrinsic.intrinsic_matrix)

    dirname = sys.argv[2]
    with open(dirname + "associations.txt", "r") as f:
        line = f.readline()
        lines = f.readlines()
        # print (lines)

    timestamps = []
    Rts = []
    Rts.append(np.eye(4,4))

    elapsed_times = []
    vis = Visualizer()
    vis.create_window()

    filename = line.replace('\n', '').split(" ")
    timestamps.append(filename[0])

    target_color = read_image(dirname + filename[1])
    target_depth = read_image(dirname + filename[3])
    target_rgbd_image = create_rgbd_image_from_tum_format(
            target_color, target_depth)

    cv2.imshow("image", cv2.cvtColor(np.array(target_color), cv2.COLOR_RGB2BGR))
    cv2.imshow("depth", np.array(target_depth))
    # cv2.waitKey(0)

    option = OdometryOption()
    odo_init = np.identity(4)
    print(option)

    flip_transform = [[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]]

    for num, line in enumerate(lines, start = 1):
        start = time.time()
        print("frame:", num)

        filename = line.replace('\n', '').split(" ") #depthのファイル名に\nが含まれていた
        timestamps.append(filename[0])

        source_color = read_image(dirname + filename[1])
        source_depth = read_image(dirname + filename[3])
        source_rgbd_image = create_rgbd_image_from_tum_format(source_color, source_depth);
        target_pcd = create_point_cloud_from_rgbd_image(
                target_rgbd_image, pinhole_camera_intrinsic)
        # source_rgbd_image = create_rgbd_image_from_color_and_depth(source_color, source_depth);
        # plt.subplot(1, 2, 1)
        # plt.title('TUM grayscale image')
        # plt.imshow(source_rgbd_image.color)
        # plt.subplot(1, 2, 2)
        # plt.title('TUM depth image')
        # plt.imshow(source_rgbd_image.depth)
        # plt.show() # plt.pause(.01)
        # cv2.imshow("image", np.array(source_rgbd_image.color))
        # cv2.imshow("depth", np.array(source_rgbd_image.depth))

        prevRt = Rts[-1]
        print (Rts[-1])

        # [success_color_term, trans_color_term, info] = compute_rgbd_odometry(
        #         source_rgbd_image, target_rgbd_image,
        #         pinhole_camera_intrinsic, odo_init,
        #         RGBDOdometryJacobianFromColorTerm(), option)
        # if success_color_term:
        #     Rts.append(np.dot(prevRt, trans_color_term))
        #     print("Using RGB-D Odometry")
        #     print(trans_color_term)
        #
        #     if num%30 == 1:
        #         source_pcd_color_term = create_point_cloud_from_rgbd_image(
        #                 source_rgbd_image, pinhole_camera_intrinsic)
        #         source_pcd_color_term.transform(Rts[-1])
        #         source_pcd_color_term.transform(flip_transform)
        #         mesh_frame = create_mesh_coordinate_frame(size = 0.1, origin = [0,0,0])
        #         mesh_frame.transform(Rts[-1])
        #         mesh_frame.transform(flip_transform)
        #         vis.add_geometry(mesh_frame)
        #         vis.add_geometry(source_pcd_color_term)
        #         vis.run()
        #     # draw_geometries([target_pcd, source_pcd_color_term])
        # else :
        #     trans_color_term = np.eye(4)
        #     Rts.append(np.dot(prevRt, trans_color_term))

        [success_hybrid_term, trans_hybrid_term, info] = compute_rgbd_odometry(
                source_rgbd_image, target_rgbd_image,
                pinhole_camera_intrinsic, odo_init,
                RGBDOdometryJacobianFromHybridTerm(), option)
        if success_hybrid_term:
            Rts.append(np.dot(prevRt, trans_hybrid_term))
            print("Using Hybrid RGB-D Odometry")
            print(trans_hybrid_term)

            if num%30 == 1:
                source_pcd_hybrid_term = create_point_cloud_from_rgbd_image(
                        source_rgbd_image, pinhole_camera_intrinsic)
                # source_pcd_hybrid_term = voxel_down_sample(source_pcd_hybrid_term, voxel_size = 0.02)
                source_pcd_hybrid_term.transform(Rts[-1])
                source_pcd_hybrid_term.transform(flip_transform)
                vis.add_geometry(source_pcd_hybrid_term)
                mesh_frame = create_mesh_coordinate_frame(size = 0.1, origin = [0,0,0])
                mesh_frame.transform(Rts[-1])
                mesh_frame.transform(flip_transform)
                vis.add_geometry(mesh_frame)
            # draw_geometries([target_pcd, source_pcd_hybrid_term])
        else :
            trans_hybrid_term = np.eye(4)
            Rts.append(np.dot(prevRt, trans_hybrid_term))

        # mesh_frame = create_mesh_coordinate_frame(size = 0.1, origin = [0,0,0])
        # mesh_frame.transform(Rts[-1])
        # mesh_frame.transform([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]])
        # vis.add_geometry(mesh_frame)
        vis.update_geometry()
        vis.poll_events()
        vis.update_renderer()
        # vis.run()

        elapsed_time = time.time() - start
        elapsed_times.append(elapsed_time)
        print(elapsed_time*1000,"[ms]")
        target_rgbd_image = source_rgbd_image

        cv2.imshow("image", cv2.cvtColor(np.array(source_color), cv2.COLOR_RGB2BGR))
        cv2.imshow("depth", np.array(source_depth))
        c = cv2.waitKey(1) & 0xFF
        if c == ord('q'):
            break

    with open(dirname + sys.argv[3], "w") as f:
        for i, timestamp in enumerate(timestamps):
            Rt = Rts[i]
            rvec = cv2.Rodrigues(Rt[0:3,0:3])[0]
            alpha = cv2.norm(rvec)
            if alpha > sys.float_info.min:
                rvec = rvec / alpha
            alpha = 0.5 * alpha
            cos_a = math.cos(alpha)
            sin_a = math.sin(alpha)
            rvec = rvec*sin_a
            f.write(str(timestamp) + " " + " ".join(map(str, Rt[0:3,3])) + " " + " ".join(map(str,rvec[0,0:3])) + " " + str(cos_a) + " " + "\n")
    vis.destroy_window()
    print("mean:",np.mean(elapsed_times)*1000,"[ms]")
~~~

2．各シーンについて、カメラの軌跡を求めます。

~~~python
# rgbd_odometry_tum_auto.py
import subprocess
import glob

dirnames = glob.glob("D:/TUM/*/")
# python rgbd_odometry_tum.py "../../TestData/camera_tum_1.json" "D:/TUM/rgbd_dataset_freiburg1_xyz/" "HybridRGB-DOdometryResult.txt"
for dirname in dirnames:
    res = subprocess.call("python rgbd_odometry_tum.py ../../TestData/camera_tum_default.json " + dirname + " HybridRGB-DOdometryResult.txt", shell=True)
~~~

# 評価
1．ATE(絶対軌跡誤差)を求めるには、下記コマンド（例）でrgbd_benchmark_toolsのevaluate_ate.pyを実行します。Python2.7.xを用います。

~~~
python evaluate_ate.py D:\TUM\rgbd_dataset_freiburg1_360\groundtruth.txt D:\TUM\rgbd_dataset_freiburg1_360\HybridRGB-DOdometryResult.txt --plot D:\TUM\rgbd_dataset_freiburg1_360\HybridRGB-DOdometryResult.png --verbose > D:\TUM\rgbd_dataset_freiburg1_360\HybridRGB-DOdometryEval.txt
~~~

2．各シーンについてATEを求めます。

~~~python
# evaluate_ate_auto.py
import subprocess
import glob

dirnames = glob.glob("D:/TUM/*/")

# python evaluate_ate.py D:\TUM\rgbd_dataset_freiburg1_desk\groundtruth.txt D:\TUM\rgbd_dataset_freiburg1_desk\HybridRGB-DOdometryResult.txt --plot D:\TUM\rgbd_dataset_freiburg1_desk\HybridRGB-DOdometryResult.png --verbose > D:\TUM\rgbd_dataset_freiburg1_desk\HybridRGB-DOdometryEval.txt
for dirname in dirnames:
    print(dirname)
    groundtruth = dirname + "groundtruth.txt"
    result = dirname + "HybridRGB-DOdometryResult.txt"
    png = dirname + "HybridRGB-DOdometryResult.png"
    evaluation = dirname + "HybridRGB-DOdometryEval.txt"
    res = subprocess.call("python evaluate_ate.py " + groundtruth + " " + result + " --plot " + png + " --verbose > " + evaluation, shell=True )
~~~

3．各シーンのATEの結果です。

| a | b | c |
|:-:|:-:|:-:|
| freiburg1_360 | freiburg1_desk | freiburg1_desk2  | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/79f80901-b5cd-6d38-6e81-6765030a1ed1.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/544f0d13-570c-342e-d7de-d83156bd72e3.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/8f280754-0721-b571-9fb4-5955dc4bfced.png) |
| freiburg1_floor  | freiburg1_plant  | freiburg1_room  |
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/234bdf9e-62a8-a6b4-8433-74fe8b2ed442.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/64131467-b304-01a3-4e42-acff60b393fa.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/2fb898f3-77d3-cfa9-ebeb-41f8753c1c95.png) | 
| freiburg1_rpy  | freiburg1_teddy  | freiburg1_xyz | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/d5addfa3-a73e-64f5-b9b6-5f1e13e64471.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/be63d0bd-11e3-f797-07d6-8e8a0d17577f.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/0f6e1dde-e5d5-d39d-1bf7-365fa56fd7ab.png) |


| d | e | f |
|:-:|:-:|:-:|
| freiburg2_360_hemisphere  | freiburg2_360_kidnap  | freiburg2_desk  |
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/43a7e16b-5c92-8ef2-11a7-1cb3f1cb0052.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/e240917d-5ecd-9f51-5455-2b43b769c18d.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/27b6a23d-546c-8748-6111-a4d868666221.png) |
| freiburg2_desk_with_person  | freiburg2_large_no_loop  | freiburg2_large_with_loop  |
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/fccd8de7-ee2c-de69-59f2-b6286f36a71d.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/f03e87c8-32ba-e4b7-9a96-f865dd811c43.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/c1cd529f-b08c-f4bd-17f6-1b54364424ce.png) |
| freiburg2_pioneer_360 | freiburg2_pioneer_slam  | freiburg2_pioneer_slam2  | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/5790a3a2-cff7-1336-da63-3223e9dd6946.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/9a769ff4-85a5-0bd7-311c-fe5af3e1abc8.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/f200dd71-250f-ac22-365c-57a98ffd53bb.png) |
| freiburg2_pioneer_slam3  | freiburg2_rpy  | freiburg2_xyz  | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/46731176-2f33-d5bf-3fde-50d849482f1d.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/256abab0-cc13-f949-1ac9-1abb007c7584.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/e86ffc88-a163-7b1b-1995-6c749ef1c644.png) |

| g | h | i |
|:-:|:-:|:-:|
| freiburg3_cabinet  | freiburg3_large_cabinet  | freiburg3_long_office_household  |
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/917cd55e-6f7b-772c-2788-c969d1c503d4.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/1b40278b-ea6a-2b98-b89a-ba98fe57e19e.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/4b344f90-5286-df24-5782-b28715dc95ac.png) |
| freiburg3_nostructure_notexture_far  | freiburg3_nostructure_texture_far  | freiburg3_nostructure_texture_far  |
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/aa9b4be2-af1e-02da-90f4-86844c46c94b.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/0d0fbfc6-f20b-c504-7f1d-20f578b6351a.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/b83093ac-5d1c-d963-a99a-dbcf8644a1b8.png) | 
| freiburg3_nostructure_texture_near_withloop  | freiburg3_sitting_halfsphere  | freiburg3_sitting_rpy  |
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/c7c5f366-9b43-4f70-48a6-e86ac8e5751a.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/5c87335e-8344-5b84-0de6-fd944c05cdac.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/96cbd2df-cd37-69eb-0fb8-1cf616a47e1e.png) | 
| freiburg3_sitting_static  | freiburg3_sitting_xyz  | freiburg3_structure_notexture_far  | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/143cfec5-21bf-d5b5-9542-533cdecfbe44.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/fc218a36-4e45-0477-c1b1-baeae42bdeb9.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/fd9636c4-6247-d0f5-534f-6fcba6f28a7a.png) | 
| freiburg3_structure_notexture_near  | freiburg3_structure_texture_far  | freiburg3_structure_texture_near  | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/d1997896-c69f-4709-e7ac-303870089202.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/a642d55e-bf9e-ef25-6eb5-292ad4053d83.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/98f4bbda-54a5-7f40-2413-8a5cc0aafde4.png) | 
| freiburg3_teddy  | freiburg3_walking_halfsphere  | freiburg3_walking_rpy  | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/594bebb4-b286-7757-21ea-22ec0cc6f233.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/5ea01207-7b07-80d6-4120-e1ad3c00b02f.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/bec7b3c1-a57f-a7c2-d014-b048d52b5cb9.png) | 
| freiburg3_walking_static  | freiburg3_walking_xyz  |   | 
| ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/62aaca24-e602-1865-b176-cc9867fa6fcf.png) | ![HybridRGB-DOdometryResult.png](https://qiita-image-store.s3.amazonaws.com/0/63863/3428c82e-ec0f-955d-f3a6-91300b177220.png) |   | 

## まとめ
- TUM RGB-D SLAM Dataset and Benchmarkの導入をしました。
- Open3DのRGB-D Odometryを用いてカメラの軌跡を求めるプログラムを作成しました。
- 評価ツールを用いて、ATEの結果をまとめました。
- これでSLAMの評価ができるようになりました。