この投稿はOpenCV Advent Calendar 2018の10日目の記事です。

はじめに

この記事では、OpenCV 4.0からopencv_contribのrgbdモジュールに追加されたcv::kinfu::KinFUを使用して、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は、KinectなどのDepthカメラを用いてリアルタイムかつ高精度に3次元形状を再構成することを目的とした手法です。Microsoft Researchの研究者らがISMAR 2011で発表しました。¹のちに、Kinect for Windows SDKに実装され開発者が利用できるようになりました。

OpenCV ContribとRGB-Dモジュールとは？

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

ここでは、RGB-Dモジュールに含まれるKinect Fusion（cv::kinfu::KinFU）をRealSenseで動かしてみましょう。
このKinect Fusionは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

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

RealSense SDKをダウンロード、インストールする。
OpenCV、OpenCV Contribのソースコードをダウンロード、バグを修正する。
- Pull Request #13341 | OpenCV
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) ²
  - OPENCV_EXTRA_MODULES_PATH <path-to-opencv_contrib>/modules ³
- WITH
  - WITH_OPENCL ☑(check)
  - WITH_LIBREALSENSE ☑(check)
生成したOpenCVのプロジェクトをビルド、インストールする。

RealSenseでcv::kinfu::KinFuを使用する

#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

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

RGB-Dモジュールのヘッダをインクルードします。

(2) Set Optimized

// (2) Set Optimized
cv::setUseOptimized( true );

最適化フラグをtrueに設定します。（OpenCV 4.0においても意味があるのか未調査）

(3) Open Video Capture

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

cv::VideoCapture()にcv::VideoCaptureAPIs::CAP_INTELPERCを指定することでRealSenseからキャプチャします。

(4) Retrieve Camera Parameters

// (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をMergeしないとこれらのパラメータの取得ができないことに注意してください。

(5) Initialize KinFu Parameters

// (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のパラメータcv::kinfu::Paramsを設定します。
パラメータは、精細だが処理の重いデフォルトのパラメータ（cv::kinfu::Params::defaultParams()）と処理が軽いが粗い（cv::kinfu::Params::coarseParams()）があらかじめ用意されています。これらのパラメータは自分で調整することもできます。

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

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

(6) Create KinFu

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

Kinect Fusionの機能を提供するcv::kinfu::KinFuを作成します。
(5)で設定したパラメータをcv::kinfu::KinFu::create()に渡して作成します。

(7) Grab All Frames and Retrieve Depth Frame

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

cv::UMat frame;
capture.retrieve( frame, cv::CAP_INTELPERC_DEPTH_MAP );

cv::VideoCapture::grab()を呼ぶことで、フレームの取得を待ちます。
cv::VideoCapture::retrieve()にcv::CAP_INTELPERC_DEPTH_MAPを指定してcv::UMatにDepthフレームを取得します。

(8) Flip Image

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

（この処理は必ずしも必要ではありませんが、）センサーを動かす方向と表示を合わせるためにフレームを水平方向に反転させます。

(9) Update Frame

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

cv::kinfu::KinFu::update()に新たにフレームを渡してKinect Fusionの3次元形状の再構成を更新します。
更新が成功した場合はtrue、失敗した場合はfalseが返されます。失敗した場合は、cv::kinfu::KinFu::reset()でKinect Fusionの3次元形状の再構成をリセットします。

(10) Rendering

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

cv::kinfu::KinFu::render()でKinect Fusionのレンダリング画像を取得します。

(11) Show Image

// (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次元形状の再構成をリセットするようにします。

実行結果

OpenCV 4.0のKinect Fusion

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

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

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

おわりに

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

KinectFusion Project Page | Microsoft Research ↩
Kinect Fusionは特許が取得されているため、NONFREEを有効にします。 ↩
C:/opencv_contrib-4.0.0/modulesなどOpenCV Contribを展開したパスを指定します。 ↩