i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added CameraK4W2 (Kinect for Windows SDK v2 driver)

Browse Source
This commit is contained in:
matlabbe
2017-11-06 00:19:12 -05:00
parent 65e82eb37e
commit 7dabfdc207
14 changed files with 912 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
CMakeLists.txt
View File

@@ -148,6 +148,7 @@ option(WITH_QT "Include Qt support" ON)
ENDIF() ENDIF()
option(WITH_FREENECT "Include Freenect support" ON) option(WITH_FREENECT "Include Freenect support" ON)
option(WITH_FREENECT2 "Include Freenect2 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_OPENNI2 "Include OpenNI2 support" ON)
option(WITH_DC1394 "Include dc1394 support" ON) option(WITH_DC1394 "Include dc1394 support" ON)
option(WITH_G2O "Include g2o support" ON) option(WITH_G2O "Include g2o support" ON)
@@ -265,6 +266,13 @@ IF(WITH_FREENECT2)
ENDIF(freenect2_FOUND) ENDIF(freenect2_FOUND)
ENDIF(WITH_FREENECT2) 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 PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
IF(WITH_OPENNI2 OR OpenNI2_FOUND) IF(WITH_OPENNI2 OR OpenNI2_FOUND)
FIND_PACKAGE(OpenNI2 QUIET) FIND_PACKAGE(OpenNI2 QUIET)
@@ -494,6 +502,11 @@ IF(NOT freenect2_FOUND)
ELSE() ELSE()
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES}) SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
ENDIF() ENDIF()
IF(NOT KinectSDK2_FOUND)
SET(K4W2 "//")
ELSE()
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES})
ENDIF()
IF(NOT OpenNI2_FOUND) IF(NOT OpenNI2_FOUND)
SET(OPENNI2 "//") SET(OPENNI2 "//")
ELSE() ELSE()
@@ -784,6 +797,14 @@ ELSE()
MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)") MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)")
ENDIF() 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) IF(DC1394_FOUND)
MESSAGE(STATUS " With dc1394 = YES (License: LGPL)") MESSAGE(STATUS " With dc1394 = YES (License: LGPL)")
ELSEIF(NOT WITH_DC1394) ELSEIF(NOT WITH_DC1394)

1
Version.h.in
View File

@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@OPENNI2@#define RTABMAP_OPENNI2 @OPENNI2@#define RTABMAP_OPENNI2
@FREENECT@#define RTABMAP_FREENECT @FREENECT@#define RTABMAP_FREENECT
@FREENECT2@#define RTABMAP_FREENECT2 @FREENECT2@#define RTABMAP_FREENECT2
@K4W2@#define RTABMAP_K4W2
@CVSBA@#define RTABMAP_CVSBA @CVSBA@#define RTABMAP_CVSBA
@POINTMATCHER@#define RTABMAP_POINTMATCHER @POINTMATCHER@#define RTABMAP_POINTMATCHER
@DC1394@#define RTABMAP_DC1394 @DC1394@#define RTABMAP_DC1394

183
cmake_modules/FindKinectSDK2.cmake Normal file
View File

@@ -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}")

47
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -77,6 +77,12 @@ namespace rs
typedef struct _freenect_context freenect_context; typedef struct _freenect_context freenect_context;
typedef struct _freenect_device freenect_device; 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 namespace rtabmap
{ {
@@ -295,6 +301,47 @@ private:
#endif #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 // CameraRealSense
///////////////////////// /////////////////////////

19
corelib/src/CMakeLists.txt
View File

@@ -96,6 +96,14 @@ SET(SRC_FILES
) )
ENDIF(OpenCV_VERSION_MAJOR EQUAL 2) 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 SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/utilite/include ${PROJECT_SOURCE_DIR}/utilite/include
${CMAKE_CURRENT_SOURCE_DIR}/../include ${CMAKE_CURRENT_SOURCE_DIR}/../include
@@ -148,6 +156,17 @@ IF(freenect2_FOUND)
) )
ENDIF(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) IF(RealSense_FOUND)
SET(INCLUDE_DIRS SET(INCLUDE_DIRS
${INCLUDE_DIRS} ${INCLUDE_DIRS}

380
corelib/src/CameraRGBD.cpp
View File

@@ -63,6 +63,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <libfreenect2/config.h> #include <libfreenect2/config.h>
#endif #endif
#ifdef RTABMAP_K4W2
#include <Kinect.h>
#endif
#ifdef RTABMAP_REALSENSE #ifdef RTABMAP_REALSENSE
#include <librealsense/rs.hpp> #include <librealsense/rs.hpp>
#ifdef RTABMAP_REALSENSE_SLAM #ifdef RTABMAP_REALSENSE_SLAM
@@ -2041,6 +2045,382 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
return data; return data;
} }
//
// CameraK4W2
//
#ifdef RTABMAP_K4W2
// Safe release for interfaces
template<class Interface>
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<HANDLE>(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<BYTE**>(&pColorBuffer));
}
else if (pColorRGBX_)
{
pColorBuffer = pColorRGBX_;
nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(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<float>::infinity() && p.Y != -std::numeric_limits<float>::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<int>(p.X);
pixel_y_l = static_cast<int>(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<unsigned char>(pixel_y_l, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_h, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_l, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_h, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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 // CameraRealSense
///////////////////////// /////////////////////////

1
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -169,6 +169,7 @@ protected slots:
void selectOpenniCvAsus(); void selectOpenniCvAsus();
void selectOpenni2(); void selectOpenni2();
void selectFreenect2(); void selectFreenect2();
void selectK4W2();
void selectRealSense(); void selectRealSense();
void selectStereoDC1394(); void selectStereoDC1394();
void selectStereoFlyCapture2(); void selectStereoFlyCapture2();

1
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -91,6 +91,7 @@ public:
kSrcFreenect2 = 5, kSrcFreenect2 = 5,
kSrcRealSense = 6, kSrcRealSense = 6,
kSrcRGBDImages = 7, kSrcRGBDImages = 7,
kSrcK4W2 = 8,
kSrcStereo = 100, kSrcStereo = 100,
kSrcDC1394 = 100, kSrcDC1394 = 100,

8
guilib/src/CMakeLists.txt
View File

@@ -107,6 +107,14 @@ SET(SRC_FILES
${srcs_qrc} ${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 SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include ${PROJECT_SOURCE_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/utilite/include ${PROJECT_SOURCE_DIR}/utilite/include

8
guilib/src/MainWindow.cpp
View File

@@ -424,6 +424,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2())); connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2())); connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2())); 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_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense())); connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394())); 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_kinect->setEnabled(CameraOpenNI2::available());
_ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available()); _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
_ui->actionFreenect2->setEnabled(CameraFreenect2::available()); _ui->actionFreenect2->setEnabled(CameraFreenect2::available());
_ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
_ui->actionRealSense_R200->setEnabled(CameraRealSense::available()); _ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
_ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available()); _ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available()); _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
@@ -4135,6 +4137,7 @@ void MainWindow::updateSelectSourceMenu()
_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
_ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2); _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_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
_ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense); _ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394); _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
@@ -5745,6 +5748,11 @@ void MainWindow::selectFreenect2()
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2); _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2);
} }
void MainWindow::selectK4W2()
{
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4W2);
}
void MainWindow::selectRealSense() void MainWindow::selectRealSense()
{ {
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense); _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense);

12
guilib/src/PreferencesDialog.cpp
View File

@@ -269,6 +269,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
{ {
_ui->comboBox_cameraRGBD->setItemData(5, 0, Qt::UserRole - 1); _ui->comboBox_cameraRGBD->setItemData(5, 0, Qt::UserRole - 1);
} }
if (!CameraK4W2::available())
{
_ui->comboBox_cameraRGBD->setItemData(8, 0, Qt::UserRole - 1);
}
if (!CameraRealSense::available()) if (!CameraRealSense::available())
{ {
_ui->comboBox_cameraRGBD->setItemData(6, 0, Qt::UserRole - 1); _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->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 &&
(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD || (_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD ||
_ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD ||
_ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD ||
_ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD ||
_ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD ||
_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD)); _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD));
@@ -4759,6 +4764,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
_ui->checkBox_freenect2NoiseFiltering->isChecked(), _ui->checkBox_freenect2NoiseFiltering->isChecked(),
_ui->lineEdit_freenect2Pipeline->text().toStdString()); _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) else if (driver == kSrcRealSense)
{ {
if(useRawImages) if(useRawImages)

130
guilib/src/ui/mainWindow.ui
View File

@@ -165,6 +165,7 @@
<normaloff>:/images/kinect_xbox_one.png</normaloff>:/images/kinect_xbox_one.png</iconset> <normaloff>:/images/kinect_xbox_one.png</normaloff>:/images/kinect_xbox_one.png</iconset>
</property> </property>
<addaction name="actionFreenect2"/> <addaction name="actionFreenect2"/>
<addaction name="actionKinect_for_Windows_SDK_v2"/>
</widget> </widget>
<widget class="QMenu" name="menuRealSense_R200"> <widget class="QMenu" name="menuRealSense_R200">
<property name="title"> <property name="title">
@@ -319,7 +320,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -357,7 +367,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -600,7 +619,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -621,7 +649,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -642,7 +679,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -663,7 +709,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
</layout> </layout>
@@ -681,7 +736,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
</layout> </layout>
@@ -702,7 +766,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -723,7 +796,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -747,7 +829,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -817,7 +908,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -1358,6 +1458,14 @@
<string>RealSense</string> <string>RealSense</string>
</property> </property>
</action> </action>
<action name="actionKinect_for_Windows_SDK_v2">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Kinect for Windows SDK v2</string>
</property>
</action>
</widget> </widget>
<customwidgets> <customwidgets>
<customwidget> <customwidget>

103
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-157</y> <y>-174</y>
<width>677</width> <width>677</width>
<height>2682</height> <height>2682</height>
</rect> </rect>
@@ -72,7 +72,16 @@
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -2814,6 +2823,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Images</string> <string>Images</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>Kinect for Windows SDK v2</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="0" column="1"> <item row="0" column="1">
@@ -2851,7 +2865,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item> <item>
<widget class="QStackedWidget" name="stackedWidget_rgbd"> <widget class="QStackedWidget" name="stackedWidget_rgbd">
<property name="currentIndex"> <property name="currentIndex">
<number>4</number> <number>7</number>
</property> </property>
<widget class="QWidget" name="page_32"> <widget class="QWidget" name="page_32">
<layout class="QVBoxLayout" name="verticalLayout_63"> <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3625,6 +3639,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item> </item>
</layout> </layout>
</widget> </widget>
<widget class="QWidget" name="page_72">
<layout class="QVBoxLayout" name="verticalLayout_127">
<item>
<spacer name="verticalSpacer_67">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>283</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget> </widget>
</item> </item>
</layout> </layout>
@@ -4615,7 +4646,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Directory of images (optional settings)</string> <string>Directory of images (optional settings)</string>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_93"> <layout class="QVBoxLayout" name="verticalLayout_93">
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -12764,7 +12804,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
<widget class="QWidget" name="page_54"> <widget class="QWidget" name="page_54">
<layout class="QVBoxLayout" name="verticalLayout_85"> <layout class="QVBoxLayout" name="verticalLayout_85">
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -12904,7 +12953,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget> </widget>
<widget class="QWidget" name="page_55"> <widget class="QWidget" name="page_55">
<layout class="QVBoxLayout" name="verticalLayout_86"> <layout class="QVBoxLayout" name="verticalLayout_86">
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -13062,7 +13120,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -13142,7 +13209,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -13254,7 +13330,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="margin"> <property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>

20
tools/CameraRGBD/main.cpp
View File

@@ -55,6 +55,7 @@ void showUsage()
" 7=FlyCapture2 (Bumblebee2)\n" " 7=FlyCapture2 (Bumblebee2)\n"
" 8=ZED stereo\n" " 8=ZED stereo\n"
" 9=RealSense\n" " 9=RealSense\n"
" 10=Kinect for Windows 2 SDK\n"
" Options:\n" " Options:\n"
" -rate #.# Input rate Hz (default 0=inf)\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" " -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 // last
driver = atoi(argv[i]); 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(); showUsage();
} }
} }
@@ -255,6 +256,15 @@ int main(int argc, char * argv[])
} }
camera = new rtabmap::CameraRealSense(0); 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 else
{ {
UFATAL(""); UFATAL("");
@@ -268,6 +278,12 @@ int main(int argc, char * argv[])
} }
rtabmap::SensorData data = camera->takeImage(); 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) 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.", UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",