LoginSignup
18
15

More than 5 years have passed since last update.

OpenCVAdvent Calendar 2018Day 10
@UnaNancyOwen(Tsukasa Sugiura)

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

Last updated at Posted at 2018-12-09

この投稿は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で発表しました。1のちに、Kinect for Windows SDKに実装され開発者が利用できるようになりました。

kinectfusion-project-page-2.jpg

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のビルド方法の詳細については、多くの記事があるのでそちらを参考にしてください。

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

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

  2. OpenCV、OpenCV Contribのソースコードをダウンロード、バグを修正する。

  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を使用する

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

実行結果

2018-12-05_17h31_31.png

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さんの「車窓を画像処理で何か」です。

  1. KinectFusion Project Page | Microsoft Research ↩

  2. Kinect Fusionは特許が取得されているため、NONFREEを有効にします。 ↩

  3. C:/opencv_contrib-4.0.0/modulesなどOpenCV Contribを展開したパスを指定します。 ↩

18
15
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
18
15