Drawing Point Cloud retrieve from Kinect v2 using Point Cloud Library – part.1 without Grabber

What is Grabber?

Point Cloud Library (PCL)にはGrabberと呼ばれるデータ入力のためのモジュールがいくつか用意されています。

  • OpenNI2Grabber
  • PrimeSensorからデータを入力するためのOpenNI2ベースのGrabber。

  • HDLGrabber/VLPGrabber
  • Velodyne LiDARからデータを入力するためのBoost.Asio/PCAPベースのGrabber。

  • RealSenseGrabber
  • Intel RealSenseからデータを入力するためのRealSense SDK/librealsenseベースのGrabber。

PCL with Kinect v2

PCLにはKinect v2からデータを入力するためのGrabberは用意されていません。
そこで、Kinect SDK v2をベースにKinect v2からPCLにデータを入力する2つの方法を紹介します。

  • part.1 without Grabber (this article)
  • Grabberを利用しない方法。

  • part.2 with Grabber
  • Grabberを利用する方法。

Drawing Point Cloud retrieved from Kinect v2 using Point Cloud Library without Grabber

サンプルプログラムは以下で公開しています。

Initialize

pcl::PointCloudpcl::visualization::PCLVisualizerを用意します。

pcl::PointCloudは点群データを扱うためのクラスです。
Kinect v2から取得したColorデータとDepthデータを利用するため、点群はpcl::PointXYZRGBAのフォーマットで準備します。
また、点群はDepthデータのサイズで初期化します。

pcl::visualization::PCLVisualizerは点群データを可視化するためのビューワーです。
ウィンドウの名前を与えてビューワーを作成します。

main.cpp#L46-L47

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud;

main.cpp#L153-L170

inline void initializePointCloud()
{
    // Create Point Cloud
    cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBA>>();
    cloud->width = static_cast<uint32_t>( depthWidth );
    cloud->height = static_cast<uint32_t>( depthHeight );
    cloud->points.resize( cloud->height * cloud->width );
    cloud->is_dense = false;

    // Create PCLVisualizer
    viewer = boost::make_shared<pcl::visualization::PCLVisualizer>( "Point Cloud Viewer" );
}

Update

Kinect v2のデータを点群に変換します。

Kinect v2から取得したデータはICoordinateMapperで座標系を変換します。
Colorデータは、Depth座標系の点に対応するColor座標系の色を取得します。
Depthデータは、Camera座標系の点に変換します。

データはpcl::PointXYZRGBAの点に入れます。
最後にpcl::PointXYZRGBAの点をpcl::PointCloudの点群に入れます。

この処理を1フレームの全ての点に対して行うことで、Kinect v2のデータを点群に変換することができます。

main.cpp#L223-L265

inline void updatePointCloud()
{
    // Reset Point Cloud
    cloud->clear();

    // Convert to Point Cloud
    for( int depthY = 0; depthY < depthHeight; depthY++ ){
        for( int depthX = 0; depthX < depthWidth; depthX++ ){
            pcl::PointXYZRGBA point;

            // Retrieve Mapped Coordinates
            DepthSpacePoint depthSpacePoint = { static_cast<float>( depthX ), static_cast<float>( depthY ) };
            UINT16 depth = depthBuffer[depthY * depthWidth + depthX];
            ColorSpacePoint colorSpacePoint = { 0.0f, 0.0f };
            ERROR_CHECK( coordinateMapper->MapDepthPointToColorSpace( depthSpacePoint, depth, &colorSpacePoint ) );

            // Set Color to Point
            int colorX = static_cast<int>( colorSpacePoint.X + 0.5f );
            int colorY = static_cast<int>( colorSpacePoint.Y + 0.5f );
            if( ( 0 <= colorX ) && ( colorX < colorWidth ) && ( 0 <= colorY ) && ( colorY < colorHeight ) ){
                unsigned int colorIndex = ( colorY * colorWidth + colorX ) * colorBytesPerPixel;
                point.b = colorBuffer[colorIndex + 0];
                point.g = colorBuffer[colorIndex + 1];
                point.r = colorBuffer[colorIndex + 2];
                point.a = colorBuffer[colorIndex + 3];
            }

            // Retrieve Mapped Coordinates
            CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f };
            ERROR_CHECK( coordinateMapper->MapDepthPointToCameraSpace( depthSpacePoint, depth, &cameraSpacePoint ) );

            // Set Depth to Point
            if( ( 0 <= colorX ) && ( colorX < colorWidth ) && ( 0 <= colorY ) && ( colorY < colorHeight ) ){
                point.x = cameraSpacePoint.X;
                point.y = cameraSpacePoint.Y;
                point.z = cameraSpacePoint.Z;
            }

            // Set Point to Point Cloud
            cloud->push_back( point );
        }
    }
}

Draw and Show

変換した点群をビューワーに登録、描画します。

main.cpp#L275-L281

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

main.cpp#L291-L295

inline void showPointCloud()
{
    // Update Viwer
    viewer->spinOnce();
}

Execution Result

このサンプルプログラムを実行するとKinect v2から取得したPoint Cloudがリアルタイムで表示されます。

kinect2_pointcloud

 


コメントを残す

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