cv::kinfu::KinFuでKinect FusionをRealSenseで動かしてみる

この投稿は[OpenCV Advent Calendar 2018](https://qiita.com/advent-calendar/2018/opencv)の10日目の記事です。

# はじめに
この記事では、OpenCV 4.0からopencv_contribのrgbdモジュールに追加された<code>cv::kinfu::KinFU</code>を使用して、RealSenseでKinect Fusionを動かす方法について紹介します。

## 動作確認環境
* RealSense D415/D435 (FW 5.10.13)
* Windows 10 Pro (1809)
* Visual Studio 2017 (15.9.3)
* OpenCV 4.0.0
* OpenCV Contrib 4.0.0
* RealSense SDK 2.17.0
* CMake 3.13.1

# Kinect Fusionとは？
[Kinect Fusion](https://www.microsoft.com/en-us/research/project/kinectfusion-project-page/)は、KinectなどのDepthカメラを用いてリアルタイムかつ高精度に3次元形状を再構成することを目的とした手法です。Microsoft Researchの研究者らがISMAR 2011で発表しました。[^1]のちに、Kinect for Windows SDKに実装され開発者が利用できるようになりました。

[![kinectfusion-project-page-2.jpg](https://qiita-image-store.s3.amazonaws.com/0/34834/1babd0cb-c386-c34a-c685-5233458acc28.jpeg)](
https://www.youtube.com/watch?v=k5e3-OCCq1Q)

# OpenCV ContribとRGB-Dモジュールとは？
[OpenCV Contrib](https://github.com/opencv/opencv_contrib)は、OpenCVの新しいモジュール、特別なモジュールなどを開発・提供するためのリポジトリです。（ときには、OpenCV/opencvから落ちてくることもあります。）
[RGB-D](https://github.com/opencv/opencv_contrib/tree/4.0.0/modules/rgbd)モジュールは、OpenCV Contribに含まれるモジュールの1つで、RGB-Dセンサーのための処理が実装されています。現在は、オブジェクト認識、法線推定、平面検出、自己位置・姿勢推定、3次元形状再構成などが含まれているようです。

ここでは、RGB-Dモジュールに含まれるKinect Fusion（<code>cv::kinfu::KinFU</code>）をRealSenseで動かしてみましょう。
このKinect Fusionは[kinfu_remake](https://github.com/Nerei/kinfu_remake)を元に実装され、オリジナルよりも3倍高速になっているそうです。

# RealSenseでKinect Fusionを動かしてみる
## OpenCVをビルド、インストールする
公式に配布されいてるビルド済みのOpenCVはRealSenseのサポートが有効になっていません。また、OpenCV Contribのモジュールも含まれていないため、OpenCVをソースコードからビルドしましょう。一日一回感謝のOpenCVビルドです。
OpenCVのビルド方法の詳細については、多くの記事があるのでそちらを参考にしてください。

* **[OpenCV入門【3.0対応】（3）OpenCVの環境構築（OpenCV 3.0／3.1） | Build Insider](https://www.buildinsider.net/small/opencv/003#cmake)**

ここでは、要点だけをまとめておきます。

1. RealSense SDKをダウンロード、インストールする。

2. OpenCV、OpenCV Contribのソースコードをダウンロード、バグを修正する。
   * **[Pull Request #13341 | OpenCV](https://github.com/opencv/opencv/pull/13341)** 

3. CMakeで以下の設定を行う。
    * **LIBREALSENSE**
        * **LIBREALSENSE_INCLUDE_DIR** C:/Program Files (x86)/Intel RealSense SDK 2.0/include
        * **LIBREALSENSE_LIBRARIES**  C:/Program Files (x86)/Intel RealSense SDK 2.0/lib/x86/realsense2.lib（またはC:/Program Files (x86)/Intel RealSense SDK 2.0/lib/x64/realsense2.lib）
    * **OPENCV**
        * **OPENCV_ENABLE_NONFREE**  ☑(check) [^2]
        * **OPENCV_EXTRA_MODULES_PATH** \<path-to-opencv_contrib\>/modules [^3]
    * **WITH**
        * **WITH_OPENCL** ☑(check)
        * **WITH_LIBREALSENSE** ☑(check)

4. 生成したOpenCVのプロジェクトをビルド、インストールする。

## RealSenseでcv::kinfu::KinFuを使用する
```cpp
#include <iostream>
#include <cmath>
#include <limits>

// (1) Include Header
#include <opencv2/opencv.hpp>
#include <opencv2/rgbd.hpp>

int main( int argc, char **argv )
{
    // (2) Set Optimized
    cv::setUseOptimized( true );

    // (3) Open Video Capture
    cv::VideoCapture capture( cv::VideoCaptureAPIs::CAP_INTELPERC );
    if( !capture.isOpened() ){
        return -1;
    }

    // (4) Retrieve Camera Parameters
    const uint32_t width  = static_cast<uint32_t>( capture.get( cv::CAP_INTELPERC_DEPTH_GENERATOR  + cv::VideoCaptureProperties::CAP_PROP_FRAME_WIDTH  ) );
    const uint32_t height = static_cast<uint32_t>( capture.get( cv::CAP_INTELPERC_DEPTH_GENERATOR  + cv::VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT ) );
    const float fx = static_cast<float>( capture.get( cv::CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ ) );
    const float fy = static_cast<float>( capture.get( cv::CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT ) );
    const float cx = width  / 2.0f - 0.5f;
    const float cy = height / 2.0f - 0.5f;
    const cv::Matx33f camera_matrix = cv::Matx33f( fx, 0.0f, cx, 0.0f, fy, cy, 0.0f, 0.0f, 1.0f );

    // (5) Initialize KinFu Parameters
    cv::Ptr<cv::kinfu::Params> params;
    params = cv::kinfu::Params::defaultParams(); // Default Parameters
    //params = cv::kinfu::Params::coarseParams(); // Coarse Parameters

    params->frameSize   = cv::Size( width, height ); // Frame Size
    params->intr        = camera_matrix;             // Camera Intrinsics
    params->depthFactor = 1000.0f;                   // Depth Factor (1000/meter)

    // (6) Create KinFu
    cv::Ptr<cv::kinfu::KinFu> kinfu;
    kinfu = cv::kinfu::KinFu::create( params );

    while( true ){
        // (7) Grab All Frames and Retrieve Depth Frame
        capture.grab();

        cv::UMat frame;
        capture.retrieve( frame, cv::CAP_INTELPERC_DEPTH_MAP );
        if( frame.empty() ){
            continue;
        }

        // (8) Flip Image
        cv::flip( frame, frame, 1 );

        // (9) Update Frame
        if( !kinfu->update( frame ) ){
            std::cout << "reset" << std::endl;
            kinfu->reset();
            continue;
        }

        // (10) Rendering
        cv::UMat render;
        kinfu->render( render );

        // (11) Show Image
        cv::imshow( "Kinect Fusion", render );
        const int32_t key = cv::waitKey( 1 );
        if( key == 'r' ){
            kinfu->reset();
        }
        if( key == 'q' ){
            break;
        }
    }

    cv::destroyAllWindows();

    return 0;
}
```

### (1) Include Header
```cpp
// (1) Include Header
#include <opencv2/opencv.hpp>
#include <opencv2/rgbd.hpp>
```
RGB-Dモジュールのヘッダをインクルードします。

### (2) Set Optimized
```cpp
// (2) Set Optimized
cv::setUseOptimized( true );
```
最適化フラグを<code>true</code>に設定します。（OpenCV 4.0においても意味があるのか未調査）

### (3) Open Video Capture
```cpp
// (3) Open Video Capture
cv::VideoCapture capture( cv::VideoCaptureAPIs::CAP_INTELPERC );
if( !capture.isOpened() ){
    return -1;
}
```
<code>cv::VideoCapture()</code>に<code>cv::VideoCaptureAPIs::CAP_INTELPERC</code>を指定することでRealSenseからキャプチャします。

### (4) Retrieve Camera Parameters
```cpp
// (4) Retrieve Camera Parameters
const uint32_t width  = static_cast<uint32_t>( capture.get( cv::CAP_INTELPERC_DEPTH_GENERATOR  + cv::VideoCaptureProperties::CAP_PROP_FRAME_WIDTH  ) );
const uint32_t height = static_cast<uint32_t>( capture.get( cv::CAP_INTELPERC_DEPTH_GENERATOR  + cv::VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT ) );
const float fx = static_cast<float>( capture.get( cv::CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ ) );
const float fy = static_cast<float>( capture.get( cv::CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT ) );
const float cx = width  / 2.0f - 0.5f;
const float cy = height / 2.0f - 0.5f;
const cv::Matx33f camera_matrix = cv::Matx33f( fx, 0.0f, cx, 0.0f, fy, cy, 0.0f, 0.0f, 1.0f );
```
Kinect Fusionに設定する各種カメラパラメータを取得します。
RealSenseでは、[#13341](https://github.com/opencv/opencv/pull/13341)をMergeしないとこれらのパラメータの取得ができないことに注意してください。

### (5) Initialize KinFu Parameters
```cpp
// (5) Initialize KinFu Parameters
cv::Ptr<cv::kinfu::Params> params;
params = cv::kinfu::Params::defaultParams(); // Default Parameters
//params = cv::kinfu::Params::coarseParams(); // Coarse Parameters

params->frameSize   = cv::Size( width, height ); // Frame Size
params->intr        = camera_matrix;             // Camera Intrinsics
params->depthFactor = 1000.0f;                   // Depth Factor (1000/meter)
```
Kinect Fusionのパラメータ<code>cv::kinfu::Params</code>を設定します。
パラメータは、精細だが処理の重いデフォルトのパラメータ（<code>cv::kinfu::Params::defaultParams()</code>）と処理が軽いが粗い（<code>cv::kinfu::Params::coarseParams()</code>）があらかじめ用意されています。これらのパラメータは自分で調整することもできます。

また、<code>cv::kinfu::Params::frameSize</code>、<code>cv::kinfu::Params::intr</code>、<code>cv::kinfu::Params::depthFactor</code>の3つのパラメータはセンサーや設定によって異なるため、それぞれ設定する必要があります。

<code>cv::kinfu::Params::frameSize</code>は、入力するDepthデータのサイズ（幅、高さ）です。
<code>cv::kinfu::Params::intr</code>は、(4)で取得したカメラの内部パラメータです。
<code>cv::kinfu::Params::depthFactor</code>は、入力するDepthデータの1mあたりの値です。RealSenseはmm単位で格納されているため、1000を設定します。

### (6) Create KinFu
```cpp
// (6) Create KinFu
cv::Ptr<cv::kinfu::KinFu> kinfu;
kinfu = cv::kinfu::KinFu::create( params );
```
Kinect Fusionの機能を提供する<code>cv::kinfu::KinFu</code>を作成します。
(5)で設定したパラメータを<code>cv::kinfu::KinFu::create()</code>に渡して作成します。

### (7) Grab All Frames and Retrieve Depth Frame
```cpp
// (7) Grab All Frames and Retrieve Depth Frame
capture.grab();

cv::UMat frame;
capture.retrieve( frame, cv::CAP_INTELPERC_DEPTH_MAP );
```
<code>cv::VideoCapture::grab()</code>を呼ぶことで、フレームの取得を待ちます。
<code>cv::VideoCapture::retrieve()</code>に<code>cv::CAP_INTELPERC_DEPTH_MAP</code>を指定して<code>cv::UMat</code>にDepthフレームを取得します。

### (8) Flip Image
```cpp
// (8) Flip Image
cv::flip( frame, frame, 1 );
```
（この処理は必ずしも必要ではありませんが、）センサーを動かす方向と表示を合わせるためにフレームを水平方向に反転させます。

### (9) Update Frame
```cpp
// (9) Update Frame
if( !kinfu->update( frame ) ){
    std::cout << "reset" << std::endl;
    kinfu->reset();
    continue;
}
```
<code>cv::kinfu::KinFu::update()</code>に新たにフレームを渡してKinect Fusionの3次元形状の再構成を更新します。
更新が成功した場合は<code>true</code>、失敗した場合は<code>false</code>が返されます。失敗した場合は、<code>cv::kinfu::KinFu::reset()</code>でKinect Fusionの3次元形状の再構成をリセットします。

### (10) Rendering
```cpp
// (10) Rendering
cv::UMat render;
kinfu->render( render );
```
<code>cv::kinfu::KinFu::render()</code>でKinect Fusionのレンダリング画像を取得します。

### (11) Show Image
```cpp
// (11) Show Image
cv::imshow( "Kinect Fusion", render );
const int32_t key = cv::waitKey( 1 );
if( key == 'r' ){
    kinfu->reset();
}
if( key == 'q' ){
    break;
}
```
Kinect Fusionのレンダリング画像を表示します。
[r]キーが押されたときに手動でKinect Fusionの3次元形状の再構成をリセットするようにします。

## 実行結果
![2018-12-05_17h31_31.png](https://qiita-image-store.s3.amazonaws.com/0/34834/a693b403-94cf-f84e-c3cb-ffd6191d7744.png)

# OpenCV 4.0のKinect Fusion
Kinect Fusionで3次元形状の再構成をすることができました。
ここでは最小限の紹介に留めましたが、<code>cv::kinfu::KinFu::getPoints()</code>などを利用して点群データを取得することもできます。

RealSenseのようなKinect以外のセンサーでもKinect Fusionを使用することができます。
すでにお気づきだと思いますが、センサーは必ずしも<code>cv::VideoCapture()</code>でサポートされている必要はありません。<code>cv::Mat</code>（<code>cv::UMat</code>）に格納できさえすれば、Depthデータの含まれる画像ファイルからでも3次元形状を再構成をすることができます。

なお、現在の実装では形状のみで色を付けることができません。
このあたりは、これから実装されるのではないでしょうか？（実装したらぷるりくを送りましょう！）

# おわりに
この記事では、OpenCV ContribのRGB-DモジュールでKinect FusionをRealSenseで動かす方法を紹介しました。
明日はKazuhitoさんの「車窓を画像処理で何か」です。

[^1]: [KinectFusion Project Page | Microsoft Research](https://www.microsoft.com/en-us/research/project/kinectfusion-project-page/)
[^2]: Kinect Fusionは特許が取得されているため、NONFREEを有効にします。
[^3]: C:/opencv_contrib-4.0.0/modulesなどOpenCV Contribを展開したパスを指定します。