mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Added CameraK4W2 (Kinect for Windows SDK v2 driver)
This commit is contained in:
matlabbe
@@ -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}
|
||||
|
||||
@@ -63,6 +63,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <libfreenect2/config.h>
|
||||
#endif
|
||||
|
||||
#ifdef RTABMAP_K4W2
|
||||
#include <Kinect.h>
|
||||
#endif
|
||||
|
||||
#ifdef RTABMAP_REALSENSE
|
||||
#include <librealsense/rs.hpp>
|
||||
#ifdef RTABMAP_REALSENSE_SLAM
|
||||
@@ -2041,6 +2045,382 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
|
||||
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
|
||||
/////////////////////////
|
||||
|
||||
Reference in New Issue
Block a user