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

What is Grabber?

Point Cloud Library (PCL) have some modules for input data that called “Grabber”.

  • OpenNI2Grabber
  • This module is grabber for input data from PrimeSensor based on OpenNI2.

  • HDLGrabber/VLPGrabber
  • This module is grabber for input data from Velodyne LiDAR based on Boost.Asio and PCAP.

  • RealSenseGrabber
  • This module is grabber for input data from Intel RealSense cameras based on RealSense SDK and librealsense.

PCL with Kinect v2

PCL doesn’t have grabber for input data from Kinect v2.
So, I will introduce two ways to input data from Kinect v2 based on Kinect SDK v2.

  • part.1 without Grabber (this article)
  • This method doesn’t use Grabber.
    It will manually inputting data to PCL from Kinect v2.

  • part.2 with Grabber
  • This method use Grabber.
    It will use Kinect2Grabber.

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

This sample program is published in following.

Initialize

First, You create pcl::PointCloud and pcl::visualization::PCLVisualizer.

pcl::PointCloud is basic Point Cloud data in PCL.
This Point Cloud create in pcl::PointXYZRGBA data format, in order to use Color data and Depth data retrieved from Kinect v2.
In addition, This Point Cloud initialize with Depth data size.

pcl::visualization::PCLVisualizer is viewer for visualize the Point Cloud data.
You create viewer by giving window name to constructor.

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

You need retrieve Color data and Depth data from Kinect v2.
Then, You will convert to Point Cloud data from Kinect v2 data.

The Kinect v2 data will convert coordinate system using ICoordinateMapper.
You will retrieve color data that matches Depth coordinate system.
And, You will converting to Camera coordinate system point from Depth data.

Then, You will input converted data to pcl::PointXYZRGBA point.
In Addition, You will input point to pcl::PointCloud<pcl::PointXYZRGBA> point cloud.

It can convert to Point Cloud data from Kinect v2 data by running this convert process for all points of one frame.

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

You will register point cloud data to viewer and draw once.

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

It will be real-time display Point Cloud that retrieved from Kinect v2 when running this sample program.

kinect2_pointcloud


Leave a Reply

Your email address will not be published. Required fields are marked *