Create a gist now

Instantly share code, notes, and snippets.

Drawing the Point Cloud retrieved from Kinect v2 using Point Cloud Library without Grabber
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()
#.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}")
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment