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

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
set( CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" ${CMAKE_MODULE_PATH} )

project( sample )
add_executable( sample main.cpp )
set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "sample" )

# Find Packages
find_package( PCL 1.8 REQUIRED )
find_package( KinectSDK2 REQUIRED )

if( PCL_FOUND AND KinectSDK2_FOUND )
  # Additional Include Directories
  include_directories( ${PCL_INCLUDE_DIRS} )
  include_directories( ${KinectSDK2_INCLUDE_DIRS} )

  # Preprocessor Definitions
  add_definitions( ${PCL_DEFINITIONS} )

  # Additional Library Directories
  link_directories( ${PCL_LIBRARY_DIRS} )
  link_directories( ${KinectSDK2_LIBRARY_DIRS} )

  # Additional Dependencies
  target_link_libraries( sample ${PCL_LIBRARIES} )
  target_link_libraries( sample ${KinectSDK2_LIBRARIES} )
endif()

FindKinectSDK2.cmake

#.rst:
# FindKinectSDK2
# --------------
#
# Find Kinect for Windows SDK v2 (Kinect SDK v2) include dirs, library dirs, libraries and post-build commands
#
# Use this module by invoking find_package with the form::
#
#    find_package( KinectSDK2 [REQUIRED] )
#
# Results for users are reported in following variables::
#
#    KinectSDK2_FOUND                - Return "TRUE" when Kinect SDK v2 found. Otherwise, Return "FALSE".
#    KinectSDK2_INCLUDE_DIRS         - Kinect SDK v2 include directories. (${KinectSDK2_DIR}/inc)
#    KinectSDK2_LIBRARY_DIRS         - Kinect SDK v2 library directories. (${KinectSDK2_DIR}/Lib/x86 or ${KinectSDK2_DIR}/Lib/x64)
#    KinectSDK2_LIBRARIES            - Kinect SDK v2 library files. (${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib (If check the box of any application festures, corresponding library will be added.))
#    KinectSDK2_COMMANDS             - Copy commands of redist files for application functions of Kinect SDK v2. (If uncheck the box of all application features, this variable has defined empty command.)
#
# This module reads hints about search locations from following environment variables::
#
#    KINECTSDK20_DIR                 - Kinect SDK v2 root directory. (This environment variable has been set by installer of Kinect SDK v2.)
#
# CMake entries::
#
#    KinectSDK2_DIR                  - Kinect SDK v2 root directory. (Default $ENV{KINECTSDK20_DIR})
#    KinectSDK2_FACE                 - Check the box when using Face or HDFace features. (Default uncheck)
#    KinectSDK2_FUSION               - Check the box when using Fusion features. (Default uncheck)
#    KinectSDK2_VGB                  - Check the box when using Visual Gesture Builder features. (Default uncheck)
#
# Example to find Kinect SDK v2::
#
#    cmake_minimum_required( VERSION 2.8 )
#
#    project( project )
#    add_executable( project main.cpp )
#
#    # Find package using this module.
#    find_package( KinectSDK2 REQUIRED )
#
#    if(KinectSDK2_FOUND)
#      # [C/C++]>[General]>[Additional Include Directories]
#      include_directories( ${KinectSDK2_INCLUDE_DIRS} )
#
#      # [Linker]>[General]>[Additional Library Directories]
#      link_directories( ${KinectSDK2_LIBRARY_DIRS} )
#
#      # [Linker]>[Input]>[Additional Dependencies]
#      target_link_libraries( project ${KinectSDK2_LIBRARIES} )
#
#      # [Build Events]>[Post-Build Event]>[Command Line]
#      add_custom_command( TARGET project POST_BUILD ${KinectSDK2_COMMANDS} )
#    endif()
#
# =============================================================================
#
# Copyright (c) 2016 Tsukasa SUGIURA
# Distributed under the MIT License.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#
# =============================================================================

##### Utility #####

# Check Directory Macro
macro(CHECK_DIR _DIR)
  if(NOT EXISTS "${${_DIR}}")
    message(WARNING "Directory \"${${_DIR}}\" not found.")
    set(KinectSDK2_FOUND FALSE)
    unset(_DIR)
  endif()
endmacro()

# Check Files Macro
macro(CHECK_FILES _FILES _DIR)
  set(_MISSING_FILES)
  foreach(_FILE ${${_FILES}})
    if(NOT EXISTS "${_FILE}")
      get_filename_component(_FILE ${_FILE} NAME)
      set(_MISSING_FILES "${_MISSING_FILES}${_FILE}, ")
    endif()
  endforeach()
  if(_MISSING_FILES)
    message(WARNING "In directory \"${${_DIR}}\" not found files: ${_MISSING_FILES}")
    set(KinectSDK2_FOUND FALSE)
    unset(_FILES)
  endif()
endmacro()

# Target Platform
set(TARGET_PLATFORM)
if(NOT CMAKE_CL_64)
  set(TARGET_PLATFORM x86)
else()
  set(TARGET_PLATFORM x64)
endif()

##### Find Kinect SDK v2 #####

# Found
set(KinectSDK2_FOUND TRUE)
if(MSVC_VERSION LESS 1700)
  message(WARNING "Kinect for Windows SDK v2 supported Visual Studio 2012 or later.")
  set(KinectSDK2_FOUND FALSE)
endif()

# Options
option(KinectSDK2_FACE "Face and HDFace features" FALSE)
option(KinectSDK2_FUSION "Fusion features" FALSE)
option(KinectSDK2_VGB "Visual Gesture Builder features" FALSE)

# Root Directoty
set(KinectSDK2_DIR)
if(KinectSDK2_FOUND)
  set(KinectSDK2_DIR $ENV{KINECTSDK20_DIR} CACHE PATH "Kinect for Windows SDK v2 Install Path." FORCE)
  check_dir(KinectSDK2_DIR)
endif()

# Include Directories
set(KinectSDK2_INCLUDE_DIRS)
if(KinectSDK2_FOUND)
  set(KinectSDK2_INCLUDE_DIRS ${KinectSDK2_DIR}/inc)
  check_dir(KinectSDK2_INCLUDE_DIRS)
endif()

# Library Directories
set(KinectSDK2_LIBRARY_DIRS)
if(KinectSDK2_FOUND)
  set(KinectSDK2_LIBRARY_DIRS ${KinectSDK2_DIR}/Lib/${TARGET_PLATFORM})
  check_dir(KinectSDK2_LIBRARY_DIRS)
endif()

# Dependencies
set(KinectSDK2_LIBRARIES)
if(KinectSDK2_FOUND)
  set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib)

  if(KinectSDK2_FACE)
    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Face.lib)
  endif()

  if(KinectSDK2_FUSION)
    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Fusion.lib)
  endif()

  if(KinectSDK2_VGB)
    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.VisualGestureBuilder.lib)
  endif()

  check_files(KinectSDK2_LIBRARIES KinectSDK2_LIBRARY_DIRS)
endif()

# Custom Commands
set(KinectSDK2_COMMANDS)
if(KinectSDK2_FOUND)
  if(KinectSDK2_FACE)
    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Face/${TARGET_PLATFORM})
    check_dir(KinectSDK2_REDIST_DIR)
    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
  endif()

  if(KinectSDK2_FUSION)
    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Fusion/${TARGET_PLATFORM})
    check_dir(KinectSDK2_REDIST_DIR)
    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
  endif()

  if(KinectSDK2_VGB)
    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/VGB/${TARGET_PLATFORM})
    check_dir(KinectSDK2_REDIST_DIR)
    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
  endif()

  # Empty Commands
  if(NOT KinectSDK2_COMMANDS)
    set(KinectSDK2_COMMANDS COMMAND)
  endif()
endif()

message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")

main.cpp

#include <sstream>
#include <stdexcept>

#define NOMINMAX
#include <Windows.h>
#include <Kinect.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <wrl/client.h>
using namespace Microsoft::WRL;

// Error Check
#define ERROR_CHECK( ret )                                        \
    if( FAILED( ret ) ){                                          \
        std::stringstream ss;                                     \
        ss << "failed " #ret " " << std::hex << ret << std::endl; \
        throw std::runtime_error( ss.str().c_str() );             \
    }

class Kinect
{
private:
    // Sensor
    ComPtr<IKinectSensor> kinect;

    // Coordinate Mapper
    ComPtr<ICoordinateMapper> coordinateMapper;

    // Reader
    ComPtr<IColorFrameReader> colorFrameReader;
    ComPtr<IDepthFrameReader> depthFrameReader;

    // Color Buffer
    std::vector<BYTE> colorBuffer;
    int colorWidth;
    int colorHeight;
    unsigned int colorBytesPerPixel;

    // Depth Buffer
    std::vector<UINT16> depthBuffer;
    int depthWidth;
    int depthHeight;
    unsigned int depthBytesPerPixel;

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

public:
    // Constructor
    Kinect()
    {
        // Initialize
        initialize();
    }

    // Destructor
    ~Kinect()
    {
        // Finalize
        finalize();
    }

    // Processing
    void run(){
        while( !viewer->wasStopped() ){
            // Update Data
            update();

            // Draw Data
            draw();

            // Show Data
            show();
        }
    }

private:
    // Initialize
    void initialize()
    {
        // Initialize Sensor
        initializeSensor();

        // Initialize Color
        initializeColor();

        // Initialize Depth
        initializeDepth();

        // Initialize Point Cloud
        initializePointCloud();
    }

    // Initialize Sensor
    inline void initializeSensor()
    {
        // Open Sensor
        ERROR_CHECK( GetDefaultKinectSensor( &kinect ) );

        ERROR_CHECK( kinect->Open() );

        // Check Open
        BOOLEAN isOpen = FALSE;
        ERROR_CHECK( kinect->get_IsOpen( &isOpen ) );
        if( !isOpen ){
            throw std::runtime_error( "failed IKinectSensor::get_IsOpen( &isOpen )" );
        }

        // Retrieve Coordinate Mapper
        ERROR_CHECK( kinect->get_CoordinateMapper( &coordinateMapper ) );
    }

    // Initialize Color
    inline void initializeColor()
    {
        // Open Color Reader
        ComPtr<IColorFrameSource> colorFrameSource;
        ERROR_CHECK( kinect->get_ColorFrameSource( &colorFrameSource ) );
        ERROR_CHECK( colorFrameSource->OpenReader( &colorFrameReader ) );

        // Retrieve Color Description
        ComPtr<IFrameDescription> colorFrameDescription;
        ERROR_CHECK( colorFrameSource->CreateFrameDescription( ColorImageFormat::ColorImageFormat_Bgra, &colorFrameDescription ) );
        ERROR_CHECK( colorFrameDescription->get_Width( &colorWidth ) ); // 1920
        ERROR_CHECK( colorFrameDescription->get_Height( &colorHeight ) ); // 1080
        ERROR_CHECK( colorFrameDescription->get_BytesPerPixel( &colorBytesPerPixel ) ); // 4

        // Allocation Color Buffer
        colorBuffer.resize( colorWidth * colorHeight * colorBytesPerPixel );
    }

    // Initialize Depth
    inline void initializeDepth()
    {
        // Open Depth Reader
        ComPtr<IDepthFrameSource> depthFrameSource;
        ERROR_CHECK( kinect->get_DepthFrameSource( &depthFrameSource ) );
        ERROR_CHECK( depthFrameSource->OpenReader( &depthFrameReader ) );

        // Retrieve Depth Description
        ComPtr<IFrameDescription> depthFrameDescription;
        ERROR_CHECK( depthFrameSource->get_FrameDescription( &depthFrameDescription ) );
        ERROR_CHECK( depthFrameDescription->get_Width( &depthWidth ) ); // 512
        ERROR_CHECK( depthFrameDescription->get_Height( &depthHeight ) ); // 424
        ERROR_CHECK( depthFrameDescription->get_BytesPerPixel( &depthBytesPerPixel ) ); // 2

        // Allocation Depth Buffer
        depthBuffer.resize( depthWidth * depthHeight );
    }

    // Initialize Point Cloud
    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" );

        // Initialize camera position
        viewer->setCameraPosition( 0.0, 0.0, -2.5, 0.0, 0.0, 0.0 );

        // Add Coordinate System
        viewer->addCoordinateSystem( 0.1 );
    }

    // Finalize
    void finalize()
    {
        // Close Sensor
        if( kinect != nullptr ){
            kinect->Close();
        }
    }

    // Update Data
    void update()
    {
        // Update Color
        updateColor();

        // Update Depth
        updateDepth();

        // Update Point Cloud
        updatePointCloud();
    }

    // Update Color
    inline void updateColor()
    {
        // Retrieve Color Frame
        ComPtr<IColorFrame> colorFrame;
        const HRESULT ret = colorFrameReader->AcquireLatestFrame( &colorFrame );
        if( FAILED( ret ) ){
            return;
        }

        // Convert Format ( YUY2 -> BGRA )
        ERROR_CHECK( colorFrame->CopyConvertedFrameDataToArray( static_cast<UINT>( colorBuffer.size() ), &colorBuffer[0], ColorImageFormat::ColorImageFormat_Bgra ) );
    }

    // Update Depth
    inline void updateDepth()
    {
        // Retrieve Depth Frame
        ComPtr<IDepthFrame> depthFrame;
        const HRESULT ret = depthFrameReader->AcquireLatestFrame( &depthFrame );
        if( FAILED( ret ) ){
            return;
        }

        // Retrieve Depth Data
        ERROR_CHECK( depthFrame->CopyFrameDataToArray( static_cast<UINT>( depthBuffer.size() ), &depthBuffer[0] ) );
    }

    // Update Point Cloud
    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 Data
    void draw()
    {
        // Draw Point Cloud
        drawPointCloud();
    }

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

    // Show Data
    void show()
    {
        // Show Point Cloud
        showPointCloud();
    }

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

int main( int argc, char* argv[] )
{
    try{
        Kinect kinect;
        kinect.run();
    } catch( std::exception& ex ){
        std::cout << ex.what() << std::endl;
    }

    return 0;
}

Excuse me, I wonder where to process the point cloud, could you please tell me?
Thank you very much!
Max