From 7dabfdc207784abec5a71324e920c389266656ca Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 6 Nov 2017 00:19:12 -0500 Subject: [PATCH] Added CameraK4W2 (Kinect for Windows SDK v2 driver) --- CMakeLists.txt | 21 + Version.h.in | 1 + cmake_modules/FindKinectSDK2.cmake | 183 +++++++++ corelib/include/rtabmap/core/CameraRGBD.h | 47 +++ corelib/src/CMakeLists.txt | 19 + corelib/src/CameraRGBD.cpp | 380 ++++++++++++++++++ guilib/include/rtabmap/gui/MainWindow.h | 1 + .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/CMakeLists.txt | 8 + guilib/src/MainWindow.cpp | 8 + guilib/src/PreferencesDialog.cpp | 12 + guilib/src/ui/mainWindow.ui | 130 +++++- guilib/src/ui/preferencesDialog.ui | 103 ++++- tools/CameraRGBD/main.cpp | 20 +- 14 files changed, 912 insertions(+), 22 deletions(-) create mode 100644 cmake_modules/FindKinectSDK2.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 90a2a317..0186c442 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -148,6 +148,7 @@ option(WITH_QT "Include Qt support" ON) ENDIF() option(WITH_FREENECT "Include Freenect support" ON) option(WITH_FREENECT2 "Include Freenect2 support" ON) +option(WITH_K4W2 "Include Kinect for Windows v2 support" ON) option(WITH_OPENNI2 "Include OpenNI2 support" ON) option(WITH_DC1394 "Include dc1394 support" ON) option(WITH_G2O "Include g2o support" ON) @@ -265,6 +266,13 @@ IF(WITH_FREENECT2) ENDIF(freenect2_FOUND) ENDIF(WITH_FREENECT2) +IF(WITH_K4W2) + FIND_PACKAGE(KinectSDK2 QUIET) + IF(KinectSDK2_FOUND) + MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}") + ENDIF(KinectSDK2_FOUND) +ENDIF(WITH_K4W2) + # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2 IF(WITH_OPENNI2 OR OpenNI2_FOUND) FIND_PACKAGE(OpenNI2 QUIET) @@ -494,6 +502,11 @@ IF(NOT freenect2_FOUND) ELSE() SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES}) ENDIF() +IF(NOT KinectSDK2_FOUND) + SET(K4W2 "//") +ELSE() + SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES}) +ENDIF() IF(NOT OpenNI2_FOUND) SET(OPENNI2 "//") ELSE() @@ -784,6 +797,14 @@ ELSE() MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)") ENDIF() +IF(KinectSDK2_FOUND) +MESSAGE(STATUS " With Kinect for Windows 2 = YES (License: Apache v2 and/or GPLv2)") +ELSEIF(NOT WITH_K4W2) +MESSAGE(STATUS " With Kinect for Windows 2 = NO (WITH_K4W2=OFF)") +ELSE() +MESSAGE(STATUS " With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)") +ENDIF() + IF(DC1394_FOUND) MESSAGE(STATUS " With dc1394 = YES (License: LGPL)") ELSEIF(NOT WITH_DC1394) diff --git a/Version.h.in b/Version.h.in index 6b341230..5164e907 100644 --- a/Version.h.in +++ b/Version.h.in @@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @OPENNI2@#define RTABMAP_OPENNI2 @FREENECT@#define RTABMAP_FREENECT @FREENECT2@#define RTABMAP_FREENECT2 +@K4W2@#define RTABMAP_K4W2 @CVSBA@#define RTABMAP_CVSBA @POINTMATCHER@#define RTABMAP_POINTMATCHER @DC1394@#define RTABMAP_DC1394 diff --git a/cmake_modules/FindKinectSDK2.cmake b/cmake_modules/FindKinectSDK2.cmake new file mode 100644 index 00000000..aace2ade --- /dev/null +++ b/cmake_modules/FindKinectSDK2.cmake @@ -0,0 +1,183 @@ +#.rst: +# FindKinectSDK2 +# -------------- +# +# Find Kinect for Windows SDK v2 (Kinect SDK v2) include dirs, library dirs, libraries +# +# 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 ) +# set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "project" ) +# +# # 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}") \ No newline at end of file diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index e86b7aa2..196f9961 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -77,6 +77,12 @@ namespace rs typedef struct _freenect_context freenect_context; typedef struct _freenect_device freenect_device; +typedef struct IKinectSensor IKinectSensor; +typedef struct ICoordinateMapper ICoordinateMapper; +typedef struct _DepthSpacePoint DepthSpacePoint; +typedef struct tagRGBQUAD RGBQUAD; +typedef struct IMultiSourceFrameReader IMultiSourceFrameReader; + namespace rtabmap { @@ -295,6 +301,47 @@ private: #endif }; +///////////////////////// +// CameraK4W2 +///////////////////////// + +class RTABMAP_EXP CameraK4W2 : + public Camera +{ +public: + static bool available(); + +public: + static const int cDepthWidth = 512; + static const int cDepthHeight = 424; + static const int cColorWidth = 1920; + static const int cColorHeight = 1080; + +public: + // default local transform z in, x right, y down)); + CameraK4W2(int deviceId = 0, + float imageRate = 0.0f, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraK4W2(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual SensorData captureImage(CameraInfo * info = 0); + +private: +#ifdef RTABMAP_K4W2 + IKinectSensor* pKinectSensor_; + ICoordinateMapper* pCoordinateMapper_; + DepthSpacePoint* pDepthCoordinates_; + IMultiSourceFrameReader* pMultiSourceFrameReader_; + RGBQUAD * pColorRGBX_; + INT_PTR hMSEvent; +#endif +}; + ///////////////////////// // CameraRealSense ///////////////////////// diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 73ada3a8..9cd5a869 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -96,6 +96,14 @@ SET(SRC_FILES ) ENDIF(OpenCV_VERSION_MAJOR EQUAL 2) +# to get includes in visual studio +IF(MSVC) + FILE(GLOB HEADERS + ../include/${PROJECT_PREFIX}/core/*.h + ) + SET(SRC_FILES ${SRC_FILES} ${HEADERS}) +ENDIF(MSVC) + SET(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/utilite/include ${CMAKE_CURRENT_SOURCE_DIR}/../include @@ -148,6 +156,17 @@ IF(freenect2_FOUND) ) ENDIF(freenect2_FOUND) +IF(KinectSDK2_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${KinectSDK2_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${KinectSDK2_LIBRARIES} + ) +ENDIF(KinectSDK2_FOUND) + IF(RealSense_FOUND) SET(INCLUDE_DIRS ${INCLUDE_DIRS} diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index fcac2615..9ba2bb72 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -63,6 +63,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef RTABMAP_K4W2 +#include +#endif + #ifdef RTABMAP_REALSENSE #include #ifdef RTABMAP_REALSENSE_SLAM @@ -2041,6 +2045,382 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info) return data; } +// +// CameraK4W2 +// + +#ifdef RTABMAP_K4W2 +// Safe release for interfaces +template +inline void SafeRelease(Interface *& pInterfaceToRelease) +{ + if (pInterfaceToRelease != NULL) + { + pInterfaceToRelease->Release(); + pInterfaceToRelease = NULL; + } +} +#endif + +bool CameraK4W2::available() +{ +#ifdef RTABMAP_K4W2 + return true; +#else + return false; +#endif +} + +CameraK4W2::CameraK4W2( + int deviceId, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform) +#ifdef RTABMAP_K4W2 + , + pKinectSensor_(NULL), + pCoordinateMapper_(NULL), + pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]), + pMultiSourceFrameReader_(NULL), + pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]), + hMSEvent(NULL) +#endif +{ +} + +CameraK4W2::~CameraK4W2() +{ +#ifdef RTABMAP_K4W2 + if (pDepthCoordinates_) + { + delete[] pDepthCoordinates_; + pDepthCoordinates_ = NULL; + } + + if (pColorRGBX_) + { + delete[] pColorRGBX_; + pColorRGBX_ = NULL; + } + + if (pMultiSourceFrameReader_) + { + pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent); + CloseHandle((HANDLE)hMSEvent); + hMSEvent = NULL; + } + + // done with frame reader + SafeRelease(pMultiSourceFrameReader_); + + // done with coordinate mapper + SafeRelease(pCoordinateMapper_); + + // close the Kinect Sensor + if (pKinectSensor_) + { + pKinectSensor_->Close(); + } + + SafeRelease(pKinectSensor_); +#endif +} + +bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & cameraName) +{ +#ifdef RTABMAP_K4W2 + HRESULT hr; + + hr = GetDefaultKinectSensor(&pKinectSensor_); + if (FAILED(hr)) + { + return false; + } + + if (pKinectSensor_) + { + // Initialize the Kinect and get coordinate mapper and the frame reader + + if (SUCCEEDED(hr)) + { + hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_); + } + + hr = pKinectSensor_->Open(); + + if (SUCCEEDED(hr)) + { + hr = pKinectSensor_->OpenMultiSourceFrameReader( + FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color, + &pMultiSourceFrameReader_); + + if (SUCCEEDED(hr)) + { + hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent); + } + } + } + + if (!pKinectSensor_ || FAILED(hr)) + { + UERROR("No ready Kinect found!"); + return false; + } + + return true; +#else + UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!"); + return false; +#endif +} + +bool CameraK4W2::isCalibrated() const +{ + return true; +} + +std::string CameraK4W2::getSerial() const +{ + return ""; +} + +SensorData CameraK4W2::captureImage(CameraInfo * info) +{ + SensorData data; + +#ifdef RTABMAP_K4W2 + + if (!pMultiSourceFrameReader_) + { + return data; + } + + HRESULT hr; + + //now check for frame events + HANDLE handles[] = { reinterpret_cast(hMSEvent) }; + + double t = UTimer::now(); + while((UTimer::now()-t < 5.0) && WaitForMultipleObjects(_countof(handles), handles, false, 5000) == WAIT_OBJECT_0) + { + IMultiSourceFrameArrivedEventArgs* pArgs = NULL; + + hr = pMultiSourceFrameReader_->GetMultiSourceFrameArrivedEventData(hMSEvent, &pArgs); + if (SUCCEEDED(hr)) + { + IMultiSourceFrameReference * pFrameRef = NULL; + hr = pArgs->get_FrameReference(&pFrameRef); + if (SUCCEEDED(hr)) + { + IMultiSourceFrame* pMultiSourceFrame = NULL; + IDepthFrame* pDepthFrame = NULL; + IColorFrame* pColorFrame = NULL; + + hr = pFrameRef->AcquireFrame(&pMultiSourceFrame); + if (FAILED(hr)) + { + UERROR("Failed getting latest frame."); + } + + IDepthFrameReference* pDepthFrameReference = NULL; + hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference); + if (SUCCEEDED(hr)) + { + hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); + } + SafeRelease(pDepthFrameReference); + + IColorFrameReference* pColorFrameReference = NULL; + hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference); + if (SUCCEEDED(hr)) + { + hr = pColorFrameReference->AcquireFrame(&pColorFrame); + } + SafeRelease(pColorFrameReference); + + if (pDepthFrame && pColorFrame) + { + INT64 nDepthTime = 0; + IFrameDescription* pDepthFrameDescription = NULL; + int nDepthWidth = 0; + int nDepthHeight = 0; + UINT nDepthBufferSize = 0; + UINT16 *pDepthBuffer = NULL; + + IFrameDescription* pColorFrameDescription = NULL; + int nColorWidth = 0; + int nColorHeight = 0; + ColorImageFormat imageFormat = ColorImageFormat_None; + UINT nColorBufferSize = 0; + RGBQUAD *pColorBuffer = NULL; + + // get depth frame data + hr = pDepthFrame->get_RelativeTime(&nDepthTime); + + if (SUCCEEDED(hr)) + { + hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription); + } + + if (SUCCEEDED(hr)) + { + hr = pDepthFrameDescription->get_Width(&nDepthWidth); + } + + if (SUCCEEDED(hr)) + { + hr = pDepthFrameDescription->get_Height(&nDepthHeight); + } + + if (SUCCEEDED(hr)) + { + hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer); + } + // get color frame data + + if (SUCCEEDED(hr)) + { + hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); + } + + if (SUCCEEDED(hr)) + { + hr = pColorFrameDescription->get_Width(&nColorWidth); + } + + if (SUCCEEDED(hr)) + { + hr = pColorFrameDescription->get_Height(&nColorHeight); + } + + if (SUCCEEDED(hr)) + { + hr = pColorFrame->get_RawColorImageFormat(&imageFormat); + } + + if (SUCCEEDED(hr)) + { + if (imageFormat == ColorImageFormat_Bgra) + { + hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast(&pColorBuffer)); + } + else if (pColorRGBX_) + { + pColorBuffer = pColorRGBX_; + nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); + hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast(pColorBuffer), ColorImageFormat_Bgra); + } + else + { + hr = E_FAIL; + } + } + if (SUCCEEDED(hr)) + { + //ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight, + // pColorBuffer, nColorWidth, nColorHeight, + // pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight); + + // Make sure we've received valid data + if (pCoordinateMapper_ && + pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) && + pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight)) + { + HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_); + if (SUCCEEDED(hr)) + { + cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1); + cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3); + // loop over output pixels + for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex) + { + DepthSpacePoint p = pDepthCoordinates_[colorIndex]; + // Values that are negative infinity means it is an invalid color to depth mapping so we + // skip processing for this pixel + if (p.X != -std::numeric_limits::infinity() && p.Y != -std::numeric_limits::infinity()) + { + // To avoid black lines caused by rounding pixel values, we should set 4 pixels + // At the same do mirror + int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h; + pixel_x_l = nDepthWidth-static_cast(p.X); + pixel_y_l = static_cast(p.Y); + pixel_x_h = pixel_x_l-1; + pixel_y_h = pixel_y_l+1; + + const RGBQUAD* pSrc = pColorBuffer + colorIndex; + if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_l, pixel_x_l); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth); + } + if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_h, pixel_x_l); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth); + } + if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_l, pixel_x_h); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth); + } + if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_h, pixel_x_h); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth); + } + } + } + + CameraIntrinsics intrinsics; + pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics); + CameraModel model( + intrinsics.FocalLengthX, + intrinsics.FocalLengthY, + intrinsics.PrincipalPointX, + intrinsics.PrincipalPointY, + this->getLocalTransform(), + 0, + depth.size()); + data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now()); + } + } + } + + SafeRelease(pDepthFrameDescription); + SafeRelease(pColorFrameDescription); + } + + pFrameRef->Release(); + + SafeRelease(pDepthFrame); + SafeRelease(pColorFrame); + SafeRelease(pMultiSourceFrame); + } + pArgs->Release(); + } + if (!data.imageRaw().empty()) + { + break; + } + } +#else + UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!"); +#endif + return data; +} + ///////////////////////// // CameraRealSense ///////////////////////// diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 9c0360ef..6be496d5 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -169,6 +169,7 @@ protected slots: void selectOpenniCvAsus(); void selectOpenni2(); void selectFreenect2(); + void selectK4W2(); void selectRealSense(); void selectStereoDC1394(); void selectStereoFlyCapture2(); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index e5532d7c..f7a19f43 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -91,6 +91,7 @@ public: kSrcFreenect2 = 5, kSrcRealSense = 6, kSrcRGBDImages = 7, + kSrcK4W2 = 8, kSrcStereo = 100, kSrcDC1394 = 100, diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 0f9bf6a2..214d617e 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -107,6 +107,14 @@ SET(SRC_FILES ${srcs_qrc} ) +# to get includes in visual studio +IF(MSVC) + FILE(GLOB HEADERS + ../include/${PROJECT_PREFIX}/gui/*.h + ) + SET(SRC_FILES ${SRC_FILES} ${HEADERS}) +ENDIF(MSVC) + SET(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/corelib/include ${PROJECT_SOURCE_DIR}/utilite/include diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 6cb6d5b2..4026a4a7 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -424,6 +424,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2())); connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2())); connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2())); + connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2())); connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense())); connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense())); connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394())); @@ -437,6 +438,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available()); _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available()); _ui->actionFreenect2->setEnabled(CameraFreenect2::available()); + _ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available()); _ui->actionRealSense_R200->setEnabled(CameraRealSense::available()); _ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available()); _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available()); @@ -4135,6 +4137,7 @@ void MainWindow::updateSelectSourceMenu() _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2); + _ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2); _ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense); _ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense); _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394); @@ -5745,6 +5748,11 @@ void MainWindow::selectFreenect2() _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2); } +void MainWindow::selectK4W2() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4W2); +} + void MainWindow::selectRealSense() { _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 341cf11a..a6ea16a0 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -269,6 +269,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : { _ui->comboBox_cameraRGBD->setItemData(5, 0, Qt::UserRole - 1); } + if (!CameraK4W2::available()) + { + _ui->comboBox_cameraRGBD->setItemData(8, 0, Qt::UserRole - 1); + } if (!CameraRealSense::available()) { _ui->comboBox_cameraRGBD->setItemData(6, 0, Qt::UserRole - 1); @@ -4128,6 +4132,7 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && (_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD || + _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD)); @@ -4759,6 +4764,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) _ui->checkBox_freenect2NoiseFiltering->isChecked(), _ui->lineEdit_freenect2Pipeline->text().toStdString()); } + else if (driver == kSrcK4W2) + { + camera = new CameraK4W2( + this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } else if (driver == kSrcRealSense) { if(useRawImages) diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 230ed80f..b30e791a 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -165,6 +165,7 @@ :/images/kinect_xbox_one.png:/images/kinect_xbox_one.png + @@ -319,7 +320,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -357,7 +367,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -600,7 +619,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -621,7 +649,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -642,7 +679,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -663,7 +709,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -681,7 +736,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -702,7 +766,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -723,7 +796,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -747,7 +829,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -817,7 +908,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -1358,6 +1458,14 @@ RealSense + + + true + + + Kinect for Windows SDK v2 + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index b46b0cf3..69b5bc0a 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -157 + -174 677 2682 @@ -72,7 +72,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -2814,6 +2823,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Images + + + Kinect for Windows SDK v2 + + @@ -2851,7 +2865,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 4 + 7 @@ -3625,6 +3639,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + Qt::Vertical + + + + 20 + 283 + + + + + + @@ -4615,7 +4646,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - + + 0 + + + 0 + + + 0 + + 0 @@ -12764,7 +12804,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + 0 + + + 0 + + + 0 + + 0 @@ -12904,7 +12953,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + 0 + + + 0 + + + 0 + + 0 @@ -13062,7 +13120,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -13142,7 +13209,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -13254,7 +13330,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 47846f26..9945ea0e 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -55,6 +55,7 @@ void showUsage() " 7=FlyCapture2 (Bumblebee2)\n" " 8=ZED stereo\n" " 9=RealSense\n" + " 10=Kinect for Windows 2 SDK\n" " Options:\n" " -rate #.# Input rate Hz (default 0=inf)\n" " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n" @@ -151,9 +152,9 @@ int main(int argc, char * argv[]) // last driver = atoi(argv[i]); - if(driver < 0 || driver > 9) + if(driver < 0 || driver > 10) { - UERROR("driver should be between 0 and 9."); + UERROR("driver should be between 0 and 10."); showUsage(); } } @@ -255,6 +256,15 @@ int main(int argc, char * argv[]) } camera = new rtabmap::CameraRealSense(0); } + else if (driver == 10) + { + if (!rtabmap::CameraK4W2::available()) + { + UERROR("Not built with Kinect for Windows 2 SDK support..."); + exit(-1); + } + camera = new rtabmap::CameraK4W2(0); + } else { UFATAL(""); @@ -268,6 +278,12 @@ int main(int argc, char * argv[]) } rtabmap::SensorData data = camera->takeImage(); + if (data.imageRaw().empty()) + { + printf("Cloud not get frame from the camera!\n"); + delete camera; + exit(1); + } if(data.imageRaw().cols % data.depthOrRightRaw().cols != 0 || data.imageRaw().rows % data.depthOrRightRaw().rows != 0) { UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",