Kinectで取得した点群をPCLで描画する

KinectやPCLの導入・設定方法に関しては他の解説記事をご覧ください！

ソースコード

main.cpp

// Cのmin,maxマクロを無効にする
#define NOMINMAX
// 安全でないメソッドの呼び出しでの警告を無効にする
#define _SCL_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS

#include <iostream>
#include <Windows.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/integral_image_normal.h>
#include "kinect2_grabber.h"

// 点群の型を定義しておく
typedef pcl::PointXYZ PointType;

// プロトタイプ宣言
void estimateNormal( pcl::PointCloud<PointType>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals );

void main()
{
    try{
        // ビューア
        pcl::visualization::PCLVisualizer viewer( "Point Cloud Viewer" );

        // 点群
        pcl::PointCloud<PointType>::Ptr cloud( new pcl::PointCloud<PointType> );

        // 点群の排他処理
        boost::mutex mutex;

        // 法線
        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals( new pcl::PointCloud < pcl::Normal > );

        // データの更新ごとに呼ばれる関数（オブジェクト）
        boost::function<void( const pcl::PointCloud<PointType>::ConstPtr& )> function
            = [&cloud, &mutex]( const pcl::PointCloud<PointType>::ConstPtr &new_cloud ){
            boost::mutex::scoped_lock lock( mutex );
            pcl::copyPointCloud( *new_cloud, *cloud );
        };

        // Kinect2Grabberを開始する
        pcl::Kinect2Grabber grabber;
        grabber.registerCallback( function );
        grabber.start();

        // ビューアーが終了されるまで動作する
        while ( !viewer.wasStopped() ){
            // 表示を更新する
            viewer.spinOnce();

            // 点群がある場合
            boost::mutex::scoped_try_lock lock( mutex );
            if ( ( cloud->size() != 0 ) && lock.owns_lock() ){
                // 点群を更新する
                auto ret = viewer.updatePointCloud( cloud, "cloud" );
                if ( !ret ){
                    // 更新がエラーになった場合は、未作成なので新しい点群として追加する
                    viewer.addPointCloud( cloud, "cloud" );
                }

                // 法線を推定する
                estimateNormal( cloud, cloud_normals );
                // 法線を更新する
                viewer.removePointCloud( "normals" );
                viewer.addPointCloudNormals<PointType, pcl::Normal>( cloud, cloud_normals, 100, 0.05, "normals" );
            }

            // エスケープキーが押されたら終了する
            if ( GetKeyState( VK_ESCAPE ) < 0 ){
                break;
            }
        }
    }

    catch ( std::exception& ex ){
        std::cout << ex.what() << std::endl;
    }
}

// 法線を推定する
void estimateNormal( pcl::PointCloud<PointType>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals )
{
    // 法線推定クラス
    pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>  ne;

    ne.setNormalEstimationMethod( ne.AVERAGE_DEPTH_CHANGE );
    ne.setMaxDepthChangeFactor( 0.01 );
    ne.setNormalSmoothingSize( 5.0 );
    ne.setInputCloud( cloud );
    ne.compute( *cloud_normals );
}

kinect2_grabber.hは以下のリンク先から頂きました。
https://github.com/UnaNancyOwen/KinectGrabber

実行結果例がこちら

にょろにょろ出ている線が推定された法線です。