PCLFusion – How to use Kinect Fusion in PCL

Why will not use Kinfu?

KinfuはPCLに含まれるKinect Fusionのクローンプロジェクトです。
Kinect FusionがまだKinect SDKに含まれていない頃から開発されてきました。

現在、Kinfuは長い間メンテナンスされていません。
残念ながらPCLにはKinfuのメンテナンスに割けるリソースもありません。
また、Kinfuは古いバージョンのOpenNIに依存しているため、モダンな開発環境で動作させるには多くの労力が必要でしょう。*1

*1 OpenNI 1.xはVisual Studio 2012以降では利用できません。

そんなKinfuの代替の1つとして、Kinect Fusionを利用する方法を提案します。
オープンソースではないため、アルゴリズムを改良するような変更はできません。
しかし、Kinect Fusionで再構成されたデータをPCLで利用することはできます。

PCLFusion

PCLFusionは、Kinect Fusionで再構成された3次元形状データの頂点をPCLのpcl::PointCloudにリアルタイムに変換・表示するサンプルプログラムです。
サンプルプログラムは以下で公開しています。

Initialize

Kinect Fusionの頂点データを変換して格納するpcl::PointCloud
それを表示するためのpcl::visualization::PCLVisualizerを作成します。
また、再構成処理を手動リセットするためのコールバック関数を登録します。

// Initialize Point Cloud
inline void Kinect::initializePointCloud()
{
    // Visualizer
    viewer = boost::make_shared<pcl::visualization::PCLVisualizer>( "Point Cloud Viewer" );
    viewer->registerKeyboardCallback( Kinect::keyboardCallback, this );

    // Point Cloud
    cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBA>>();
    cloud->is_dense = false;
}

// Keyboard Callback Function
void Kinect::keyboardCallback( const pcl::visualization::KeyboardEvent& event, void* cookie )
{
    // Reset Reconstruction
    if( event.getKeySym() == "r" && event.keyDown() ){
        static_cast<Kinect*>( cookie )->reset();
    }
}

Update

Kinect Fusionで再構成したボリュームデータからメッシュデータを取得します。

このときサンプリングステップを小さくしすぎると、変換処理に時間がかかりフレームレートが大きく落ちてしまいます。
リアルタイムで処理するにはステップを3以上に設定することをお勧めします。
(ボクセルグリッドフィルタのようにデータを間引く処理を行います。)

メッシュデータから頂点情報と色情報を取得して、pcl::PointCloudに変換します。
あとは煮るなり焼くなりPCLで処理することができます。

// Update Point Cloud
inline void Kinect::updatePointCloud()
{
    // Calculate Mesh Data
    const UINT step = 3;
    ComPtr<INuiFusionColorMesh> mesh;
    HRESULT ret = reconstruction->CalculateMesh( step, &mesh );
    if( FAILED( ret ) ){
        return;
    }

    // Retrieve Vertex Count
    const unsigned int verticesCount = mesh->VertexCount();
    if( !verticesCount ){
        return;
    }

    // Retrieve Vertices
    const Vector3* vertices = nullptr;
    ERROR_CHECK( mesh->GetVertices( &vertices ) );

    // Retrieve Colors
    const int* colors = nullptr;
    ERROR_CHECK( mesh->GetColors( &colors ) );

    // ReSet Point Cloud
    cloud->clear();
    cloud->width = static_cast<uint32_t>( verticesCount );
    cloud->height = static_cast<uint32_t>( 1 );
    cloud->points.resize( cloud->width * cloud->height );

    // Convert Mesh to Point Cloud
    Concurrency::parallel_for( static_cast<unsigned int>( 0 ), verticesCount, [&]( const unsigned int index ){
        pcl::PointXYZRGBA point;

        const Vector3 vertex = vertices[index];
        point.x = vertex.x;
        point.y = -vertex.y;
        point.z = -vertex.z;

        const uint32_t color = colors[index];
        point.rgba = color;

        cloud->points[index] = point;
    } );
}

Draw and Show

Point CloudをPCLVisualizerに登録、表示します。

// Draw Point Cloud
inline void Kinect::drawPointCloud()
{
    // Update Point Cloud
    if( !viewer->updatePointCloud( cloud, "cloud" ) ){
        viewer->addPointCloud( cloud, "cloud" );
    }
}

// Show Point Cloud
inline void Kinect::showPointCloud()
{
    // Spin Viewer
    viewer->spinOnce();
}

Execution Result

このサンプルプログラムを実行するとKinect Fusionで再構成された3次元形状データの頂点がリアルタイムに表示されます。
変換処理が遅れてフレームレートが落ちるようなら、サンプリングステップの設定を変更してください。


コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です