C++ Wrapper for Azure Kinect Body Tracking SDK

CMakeLists.txt

cmake_minimum_required( VERSION 3.6 )

set( CMAKE_CXX_STANDARD 11 )
set( CMAKE_CXX_STANDARD_REQUIRED ON )
set( CMAKE_CXX_EXTENSIONS OFF )

project( K4A )
add_executable( K4A k4abt.hpp util.h main.cpp )

set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "K4A" )

find_package( OpenCV REQUIRED )

set( CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" ${CMAKE_MODULE_PATH} )
find_package( k4a REQUIRED )
find_package( k4abt REQUIRED )

if( k4a_FOUND AND k4abt_FOUND AND OpenCV_FOUND )
  target_link_libraries( K4A k4a::k4a )
  target_link_libraries( K4A k4a::k4abt )
  target_link_libraries( K4A ${OpenCV_LIBS} )
endif()

Findk4a.cmake

#.rst:
# Findk4a
# -------
#
# Find Azure Kinect Sensor SDK include dirs, and libraries.
#
# IMPORTED Targets
# ^^^^^^^^^^^^^^^^
#
# This module defines the :prop_tgt:`IMPORTED` targets:
#
# ``k4a::k4a``
#  Defined if the system has Azure Kinect Sensor SDK.
#
# Result Variables
# ^^^^^^^^^^^^^^^^
#
# This module sets the following variables:
#
# ::
#
#   k4a_FOUND               True in case Azure Kinect Sensor SDK is found, otherwise false
#   k4a_ROOT                Path to the root of found Azure Kinect Sensor SDK installation
#
# Example usage
# ^^^^^^^^^^^^^
#
# ::
#
#     find_package(k4a REQUIRED)
#
#     add_executable(foo foo.cc)
#     target_link_libraries(foo k4a::k4a)
#
# License
# ^^^^^^^
#
# Copyright (c) 2019 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.
#

file(GLOB K4A_DIR_PATHS "$ENV{PROGRAMW6432}/Azure Kinect SDK v*")
list(REVERSE K4A_DIR_PATHS)

foreach(K4A_DIR_PATH IN ITEMS ${K4A_DIR_PATHS})
  find_path(k4a_INCLUDE_DIR
    NAMES
      k4a/k4a.h
    HINTS
      $ENV{K4A_DIR}/sdk/
    PATHS
      "${K4A_DIR_PATH}/sdk/"
    PATH_SUFFIXES
      include
  )

  find_library(k4a_LIBRARY
    NAMES
      k4a.lib
    HINTS
      $ENV{K4A_DIR}/windows-desktop/amd64/release
    PATHS
      "${K4A_DIR_PATH}/sdk/windows-desktop/amd64/release"
    PATH_SUFFIXES
      lib
  )

  if(NOT k4a_INCLUDE_DIR OR NOT k4a_LIBRARY)
    continue()
  endif()

  include(FindPackageHandleStandardArgs)
  find_package_handle_standard_args(
    k4a DEFAULT_MSG
    k4a_LIBRARY k4a_INCLUDE_DIR
  )

  if(k4a_FOUND)
    add_library(k4a::k4a SHARED IMPORTED)
    set_target_properties(k4a::k4a PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${k4a_INCLUDE_DIR}")

    set_property(TARGET k4a::k4a APPEND PROPERTY IMPORTED_CONFIGURATIONS "RELEASE")
    set_target_properties(k4a::k4a PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX")
    set_target_properties(k4a::k4a PROPERTIES IMPORTED_IMPLIB_RELEASE "${k4a_LIBRARY}")

    set_property(TARGET k4a::k4a APPEND PROPERTY IMPORTED_CONFIGURATIONS "DEBUG")
    set_target_properties(k4a::k4a PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX")
    set_target_properties(k4a::k4a PROPERTIES IMPORTED_IMPLIB_DEBUG "${k4a_LIBRARY}")

    get_filename_component(K4a_ROOT "${k4a_INCLUDE_DIR}" PATH)

    break()
  endif()
endforeach()

Findk4abt.cmake

#.rst:
# Findk4abt
# ---------
#
# Find Azure Kinect Body Tracking SDK include dirs, and libraries.
#
# IMPORTED Targets
# ^^^^^^^^^^^^^^^^
#
# This module defines the :prop_tgt:`IMPORTED` targets:
#
# ``k4a::k4abt``
#  Defined if the system has Azure Kinect Body Tracking SDK.
#
# Result Variables
# ^^^^^^^^^^^^^^^^
#
# This module sets the following variables:
#
# ::
#
#   k4abt_FOUND               True in case Azure Kinect Body Tracking SDK is found, otherwise false
#   k4abt_ROOT                Path to the root of found Azure Kinect Body Tracking SDK installation
#
# Example Usage
# ^^^^^^^^^^^^^
#
# ::
#
#     find_package(k4abt REQUIRED)
#
#     add_executable(foo foo.cc)
#     target_link_libraries(foo k4a::k4abt)
#
# License
# ^^^^^^^
#
# Copyright (c) 2019 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.
#

find_path(k4abt_INCLUDE_DIR
  NAMES
    k4abt.h
  HINTS
    $ENV{K4ABT_ROOT}/sdk/
  PATHS
    "$ENV{PROGRAMW6432}/Azure Kinect Body Tracking SDK/sdk/"
  PATH_SUFFIXES
    include
)

find_library(k4abt_LIBRARY
  NAMES
    k4abt.lib
  HINTS
    $ENV{K4ABT_ROOT}/sdk/windows-desktop/amd64/release
  PATHS
    "$ENV{PROGRAMW6432}/Azure Kinect Body Tracking SDK/sdk/windows-desktop/amd64/release"
  PATH_SUFFIXES
    lib
)

include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(
  k4abt DEFAULT_MSG
  k4abt_LIBRARY k4abt_INCLUDE_DIR
)

if(k4abt_FOUND)
  add_library(k4a::k4abt SHARED IMPORTED)
  set_target_properties(k4a::k4abt PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${k4abt_INCLUDE_DIR}")

  set_property(TARGET k4a::k4abt APPEND PROPERTY IMPORTED_CONFIGURATIONS "RELEASE")
  set_target_properties(k4a::k4abt PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX")
  set_target_properties(k4a::k4abt PROPERTIES IMPORTED_IMPLIB_RELEASE "${k4abt_LIBRARY}")

  set_property(TARGET k4a::k4abt APPEND PROPERTY IMPORTED_CONFIGURATIONS "DEBUG")
  set_target_properties(k4a::k4abt PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX")
  set_target_properties(k4a::k4abt PROPERTIES IMPORTED_IMPLIB_DEBUG "${k4abt_LIBRARY}")

  get_filename_component(k4abt_ROOT "${k4abt_INCLUDE_DIR}" PATH)
endif()

k4abt.hpp

/*
 This is C++ wrapper for Azure Kinect Body Tracking SDK.

 Copyright (c) 2019 Tsukasa Sugiura <t.sugiura0204@gmail.com>
 Licensed 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.
*/

#ifndef K4ABT_HPP
#define K4ABT_HPP

#include <k4a/k4a.h>
#include <k4a/k4a.hpp>
#include <k4abt.h>

#include <vector>
#include <chrono>

namespace k4abt
{
    class frame
    {
    public:
        frame( k4abt_frame_t handle = nullptr ) noexcept : m_handle( handle ) {}

        frame( const frame& other ) noexcept : m_handle( other.m_handle )
        {
            if ( m_handle != nullptr )
            {
                k4abt_frame_reference( m_handle );
            }
        }

        frame( frame&& other ) noexcept : m_handle( other.m_handle )
        {
            other.m_handle = nullptr;
        }

        ~frame()
        {
            reset();
        }

        frame& operator=( const frame& other ) noexcept
        {
            if ( this != &other )
            {
                reset();
                m_handle = other.m_handle;
                if ( m_handle != nullptr )
                {
                    k4abt_frame_reference( m_handle );
                }
            }
            return *this;
        }

        frame& operator=( std::nullptr_t ) noexcept
        {
            reset();
            return *this;
        }

        bool operator==( const frame& other ) const noexcept
        {
            return m_handle == other.m_handle;
        }

        bool operator==( std::nullptr_t ) const noexcept
        {
            return m_handle == nullptr;
        }

        bool operator!=( const frame& other ) const noexcept
        {
            return m_handle != other.m_handle;
        }

        bool operator!=( std::nullptr_t ) const noexcept
        {
            return m_handle != nullptr;
        }

        operator bool() const noexcept
        {
            return m_handle != nullptr;
        }

        k4abt_frame_t handle() const noexcept
        {
            return m_handle;
        }

        void reset() noexcept
        {
            if ( m_handle != nullptr )
            {
                k4abt_frame_release( m_handle );
                m_handle = nullptr;
            }
        }

        size_t get_num_bodies() const noexcept
        {
            return k4abt_frame_get_num_bodies( m_handle );
        }

        k4a::image get_body_index_map() const noexcept
        {
            return k4a::image( k4abt_frame_get_body_index_map( m_handle ) );
        }

        std::vector<k4abt_body_t> get_bodies() const
        {
            const int32_t num_bodies = static_cast<int32_t>( get_num_bodies() );
            std::vector<k4abt_body_t> bodies;
            for( int32_t i = 0; i < num_bodies; i++ )
            {
                k4abt_body_t body;
                k4a_result_t result = k4abt_frame_get_body_skeleton( m_handle, i, &body.skeleton );
                if ( K4A_RESULT_SUCCEEDED != result )
                {
                    throw k4a::error( "Failed to get body skeleton!" );
                }
                body.id = k4abt_frame_get_body_id( m_handle, i );
                bodies.emplace_back( body );
            }
            return bodies;
        }

        k4a::capture get_capture() const noexcept
        {
            return k4a::capture( k4abt_frame_get_capture( m_handle ) );
        }

        std::chrono::microseconds get_timestamp()
        {
            return std::chrono::microseconds( k4abt_frame_get_timestamp_usec( m_handle ) );
        }
    private:
        k4abt_frame_t m_handle;
    };

    class tracker
    {
    public:
        tracker( k4abt_tracker_t handle = nullptr ) noexcept : m_handle( handle ) {}

        tracker( tracker&& other ) noexcept : m_handle( other.m_handle )
        {
            other.m_handle = nullptr;
        }

        ~tracker()
        {
            destroy();
        }

        tracker& operator=( std::nullptr_t ) noexcept
        {
            destroy();
            return *this;
        }

        bool operator==( const tracker& other ) const noexcept
        {
            return m_handle == other.m_handle;
        }

        bool operator==( std::nullptr_t ) const noexcept
        {
            return m_handle == nullptr;
        }

        bool operator!=( const tracker& other ) const noexcept
        {
            return m_handle != other.m_handle;
        }

        bool operator!=( std::nullptr_t ) const noexcept
        {
            return m_handle != nullptr;
        }

        operator bool() const noexcept
        {
            return m_handle != nullptr;
        }

        k4abt_tracker_t handle() const noexcept
        {
            return m_handle;
        }

        void destroy() noexcept
        {
            if ( m_handle != nullptr )
            {
                k4abt_tracker_destroy( m_handle );
                m_handle = nullptr;
            }
        }

        void shutdown() noexcept
        {
            if ( m_handle != nullptr )
            {
                k4abt_tracker_shutdown( m_handle );
            }
        }

        static tracker create( k4a::calibration& calibration )
        {
            k4abt_tracker_t handle = nullptr;
            k4a_result_t result = k4abt_tracker_create( dynamic_cast<k4a_calibration_t*>( &calibration ), &handle );
            if ( K4A_RESULT_SUCCEEDED != result )
            {
                throw k4a::error( "Failed to create tracker!" );
            }
            return tracker( handle );
        }

        bool enqueue_capture( k4a::capture& capture, std::chrono::milliseconds timeout = std::chrono::milliseconds( K4A_WAIT_INFINITE ) )
        {
            k4a_wait_result_t result = k4abt_tracker_enqueue_capture( m_handle, capture.handle(), static_cast<int32_t>( timeout.count() ) );
            if ( K4A_WAIT_RESULT_TIMEOUT == result )
            {
                return false;
            }
            if ( K4A_WAIT_RESULT_SUCCEEDED != result )
            {
                throw k4a::error( "Failed to enqueue capture!" );
            }
            return true;
        }

        frame pop_result( std::chrono::milliseconds timeout = std::chrono::milliseconds( K4A_WAIT_INFINITE ) )
        {
            k4abt_frame_t body_frame = nullptr;
            k4a_wait_result_t result = k4abt_tracker_pop_result( m_handle, &body_frame, static_cast<int32_t>( timeout.count() ) );
            if ( K4A_WAIT_RESULT_TIMEOUT == result )
            {
                return frame();
            }
            if ( K4A_WAIT_RESULT_SUCCEEDED != result )
            {
                throw k4a::error( "Failed to pop result!" );
            }
            return frame( body_frame );
        }
    private:
        k4abt_tracker_t m_handle;
    };
}

#endif

main.cpp

#include <iostream>
#include <k4a/k4a.h>
#include <k4a/k4a.hpp>
#include <k4abt.h>
#include <opencv2/opencv.hpp>

// Converter Utility Header
#include "util.h"

// C++ Wrapper for Azure Kinect Body Tracking SDK
#include "k4abt.hpp"

int main( int argc, char* argv[] )
{
    try{
        // Get Connected Devices
        const int32_t device_count = k4a_device_get_installed_count();
        if( device_count == 0 ){
            throw k4a::error( "Failed to found device!" );
        }

        // Open Default Device
        k4a::device device = k4a::device::open( K4A_DEVICE_DEFAULT );

        // Start Cameras with Configuration
        k4a_device_configuration_t configuration = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
        configuration.color_format             = K4A_IMAGE_FORMAT_COLOR_BGRA32;
        configuration.color_resolution         = K4A_COLOR_RESOLUTION_720P;
        configuration.depth_mode               = K4A_DEPTH_MODE_NFOV_UNBINNED;
        configuration.synchronized_images_only = true;
        device.start_cameras( &configuration );

        // Get Calibration
        k4a::calibration calibration = device.get_calibration( configuration.depth_mode, configuration.color_resolution );

        // Create Body Tracker
        k4abt::tracker tracker = k4abt::tracker::create( calibration );
        if( !tracker ){
            throw k4a::error( "Failed to create tracker!" );
        }

        std::vector<cv::Vec3b> colors;
        colors.push_back( cv::Vec3b( 255,   0,   0 ) );
        colors.push_back( cv::Vec3b(   0, 255,   0 ) );
        colors.push_back( cv::Vec3b(   0,   0, 255 ) );
        colors.push_back( cv::Vec3b( 255, 255,   0 ) );
        colors.push_back( cv::Vec3b(   0, 255, 255 ) );
        colors.push_back( cv::Vec3b( 255,   0, 255 ) );

        while( true ){
            // Get Capture
            k4a::capture capture;
            bool result = device.get_capture( &capture, std::chrono::milliseconds( K4A_WAIT_INFINITE ) );
            if( !result ){
                break;
            }

            // Get Color Image
            k4a::image color_image = capture.get_color_image();
            cv::Mat color = k4a::get_mat( color_image );

            // Get Depth Image
            k4a::image depth_image = capture.get_depth_image();
            cv::Mat depth = k4a::get_mat( depth_image );

            // Enqueue Capture
            tracker.enqueue_capture( capture );

            // Pop Result
            k4abt::frame body_frame = tracker.pop_result();

            // Get Num Bodies
            const int32_t num_bodies = body_frame.get_num_bodies();
            std::cout << num_bodies << std::endl;

            // Get Body Index Map
            k4a::image body_index_map_image = body_frame.get_body_index_map();
            cv::Mat body_index_map = k4a::get_mat( body_index_map_image );

            // Get Body
            std::vector<k4abt_body_t> bodies = body_frame.get_bodies();

            // Clear Handle
            capture.reset();
            color_image.reset();
            depth_image.reset();

            // Scaling Depth
            depth.convertTo( depth, CV_8U, -255.0 / 5000.0, 255.0 );

            // Draw Body Index
            cv::Mat body_index = cv::Mat::zeros( body_index_map.size(), CV_8UC3 );
            body_index.forEach<cv::Vec3b>( [&]( cv::Vec3b& pixel, const int32_t* position ){
                const int32_t index = body_index_map.at<uint8_t>( position[0], position[1] );
                if( index != K4ABT_BODY_INDEX_MAP_BACKGROUND ){
                    pixel = colors[index % colors.size()];
                }
            } );

            // Draw Skeleton
            k4a::capture body_capture = body_frame.get_capture();
            k4a::image skeleton_image = body_capture.get_color_image();
            cv::Mat skeleton = k4a::get_mat( skeleton_image );
            for( const k4abt_body_t& body : bodies ){
                for( const k4abt_joint_t& joint : body.skeleton.joints ){
                    k4a_float2_t position;
                    const bool result = calibration.convert_3d_to_2d( joint.position, k4a_calibration_type_t::K4A_CALIBRATION_TYPE_DEPTH, k4a_calibration_type_t::K4A_CALIBRATION_TYPE_COLOR, &position );
                    if( !result ){
                        continue;
                    }
                    const int32_t id = body.id;
                    const cv::Point point = { static_cast<int32_t>( position.xy.x ), static_cast<int32_t>( position.xy.y ) };
                    cv::circle( skeleton, point, 5, colors[( id - 1 ) % colors.size()], -1 );
                }
            }

            // Clear Handle
            body_frame.reset();
            body_index_map_image.reset();
            body_capture.reset();
            skeleton_image.reset();

            // Show Image
            cv::imshow( "color", color );
            cv::imshow( "depth", depth );
            cv::imshow( "body index", body_index );
            cv::imshow( "skeleton", skeleton );
            const int32_t key = cv::waitKey( 30 );
            if( key == 'q' ){
                break;
            }
        }

        // Stop Body Tracker
        tracker.destroy();

        // Stop Cameras
        device.stop_cameras();

        // Close Device
        device.close();

        // Close Window
        cv::destroyAllWindows();
    }
    catch( const k4a::error& error ){
        std::cout << error.what() << std::endl;
    }

    return 0;
}

util.h

/*
 This is utility to that provides converter to convert k4a::image to cv::Mat.

 cv::Mat mat = k4a::get_mat( image );

 Copyright (c) 2019 Tsukasa Sugiura <t.sugiura0204@gmail.com>
 Licensed 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.
*/

#ifndef __UTIL__
#define __UTIL__

#include <vector>
#include <limits>

#include <k4a/k4a.h>
#include <k4a/k4a.hpp>
#include <opencv2/opencv.hpp>

namespace k4a
{
    cv::Mat get_mat( k4a::image& src, bool deep_copy = true )
    {
        assert( src.get_size() != 0 );

        cv::Mat mat;
        const int32_t width = src.get_width_pixels();
        const int32_t height = src.get_height_pixels();

        const k4a_image_format_t format = src.get_format();
        switch( format )
        {
            case k4a_image_format_t::K4A_IMAGE_FORMAT_COLOR_MJPG:
            {
                // NOTE: this is slower than other formats.
                std::vector<uint8_t> buffer( src.get_buffer(), src.get_buffer() + src.get_size() );
                mat = cv::imdecode( buffer, cv::IMREAD_ANYCOLOR );
                cv::cvtColor( mat, mat, cv::COLOR_BGR2BGRA );
                break;
            }
            case k4a_image_format_t::K4A_IMAGE_FORMAT_COLOR_NV12:
            {
                cv::Mat nv12 = cv::Mat( height + height / 2, width, CV_8UC1, src.get_buffer() ).clone();
                cv::cvtColor( nv12, mat, cv::COLOR_YUV2BGRA_NV12 );
                break;
            }
            case k4a_image_format_t::K4A_IMAGE_FORMAT_COLOR_YUY2:
            {
                cv::Mat yuy2 = cv::Mat( height, width, CV_8UC2, src.get_buffer() ).clone();
                cv::cvtColor( yuy2, mat, cv::COLOR_YUV2BGRA_YUY2 );
                break;
            }
            case k4a_image_format_t::K4A_IMAGE_FORMAT_COLOR_BGRA32:
            {
                mat = deep_copy ? cv::Mat( height, width, CV_8UC4, src.get_buffer() ).clone()
                                : cv::Mat( height, width, CV_8UC4, src.get_buffer() );
                break;
            }
            case k4a_image_format_t::K4A_IMAGE_FORMAT_DEPTH16:
            case k4a_image_format_t::K4A_IMAGE_FORMAT_IR16:
            {
                mat = deep_copy ? cv::Mat( height, width, CV_16UC1, reinterpret_cast<uint16_t*>( src.get_buffer() ) ).clone()
                                : cv::Mat( height, width, CV_16UC1, reinterpret_cast<uint16_t*>( src.get_buffer() ) );
                break;
            }
            case k4a_image_format_t::K4A_IMAGE_FORMAT_CUSTOM8:
            {
                mat = cv::Mat( height, width, CV_8UC1, src.get_buffer() ).clone();
                break;
            }
            case k4a_image_format_t::K4A_IMAGE_FORMAT_CUSTOM:
            {
                // NOTE: This is opencv_viz module format (cv::viz::WCloud).
                const int16_t* buffer = reinterpret_cast<int16_t*>( src.get_buffer() );
                mat = cv::Mat( height, width, CV_32FC3, cv::Vec3f::all( std::numeric_limits<float>::quiet_NaN() ) );
                mat.forEach<cv::Vec3f>(
                    [&]( cv::Vec3f& point, const int32_t* position ){
                        const int32_t index = ( position[0] * width + position[1] ) * 3;
                        point = cv::Vec3f( buffer[index + 0], buffer[index + 1], buffer[index + 2] );
                    }
                );
                break;
            }
            default:
                throw k4a::error( "Failed to convert this format!" );
                break;
        }

        return mat;
    }
}

cv::Mat k4a_get_mat( k4a_image_t& src, bool deep_copy = true )
{
    k4a_image_reference( src );
    return k4a::get_mat( k4a::image( src ), deep_copy );
}

#endif // __UTIL__