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

CameraK4W2: added depth to color registration. For CameraFreenect2, kTypeColor2DepthSD is now used by default.

Browse Source
This commit is contained in:
matlabbe
2017-11-06 22:59:12 -05:00
parent 7dabfdc207
commit 8cdd138143
5 changed files with 363 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

364
corelib/src/CameraRGBD.cpp
View File

@@ -2073,14 +2073,17 @@ bool CameraK4W2::available()
CameraK4W2::CameraK4W2(
int deviceId,
Type type,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_K4W2
,
type_(type),
pKinectSensor_(NULL),
pCoordinateMapper_(NULL),
pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
pMultiSourceFrameReader_(NULL),
pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
hMSEvent(NULL)
@@ -2097,12 +2100,25 @@ CameraK4W2::~CameraK4W2()
pDepthCoordinates_ = NULL;
}
if (pColorCoordinates_)
{
delete[] pColorCoordinates_;
pColorCoordinates_ = NULL;
}
if (pColorRGBX_)
{
delete[] pColorRGBX_;
pColorRGBX_ = NULL;
}
close();
#endif
}
void CameraK4W2::close()
{
#ifdef RTABMAP_K4W2
if (pMultiSourceFrameReader_)
{
pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
@@ -2123,6 +2139,8 @@ CameraK4W2::~CameraK4W2()
}
SafeRelease(pKinectSensor_);
colorCameraModel_ = CameraModel();
#endif
}
@@ -2131,6 +2149,8 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string &
#ifdef RTABMAP_K4W2
HRESULT hr;
close();
hr = GetDefaultKinectSensor(&pKinectSensor_);
if (FAILED(hr))
{
@@ -2141,22 +2161,22 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string &
{
// 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_);
hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
if (SUCCEEDED(hr))
{
hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
hr = pKinectSensor_->OpenMultiSourceFrameReader(
FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
&pMultiSourceFrameReader_);
if (SUCCEEDED(hr))
{
hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
}
}
}
}
@@ -2164,9 +2184,82 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string &
if (!pKinectSensor_ || FAILED(hr))
{
UERROR("No ready Kinect found!");
close();
return false;
}
// to query camera parameters, we should wait a little
uSleep(3000);
// initialize color calibration if not set yet
CameraIntrinsics intrinsics;
hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
{
// guess color intrinsics by comparing two reprojections
CameraModel depthModel(
intrinsics.FocalLengthX,
intrinsics.FocalLengthY,
intrinsics.PrincipalPointX,
intrinsics.PrincipalPointY);
cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
if (SUCCEEDED(hr))
{
int firstIndex = -1;
int lastIndex = -1;
for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
{
ColorSpacePoint p = pColorCoordinates_[depthIndex];
// 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())
{
if (firstIndex == -1)
{
firstIndex = depthIndex;
}
lastIndex = depthIndex;
}
}
UASSERT(firstIndex >= 0 && lastIndex >= 0);
float fx, fy, cx, cy;
float x1, y1, z1, x2, y2, z2;
depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
cx = uv1.X - (x1 / z1) * fx;
fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
cy = uv1.Y - (y1 / z1) * fy;
colorCameraModel_ = CameraModel(
fx,
fy,
float(cColorWidth) - cx,
cy,
this->getLocalTransform(),
0,
cv::Size(cColorWidth, cColorHeight));
}
}
if (!colorCameraModel_.isValidForProjection())
{
UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
close();
return false;
}
std::string serial = getSerial();
if (!serial.empty())
{
UINFO("Running kinect device \"%s\"", serial.c_str());
}
return true;
#else
UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
@@ -2181,6 +2274,19 @@ bool CameraK4W2::isCalibrated() const
std::string CameraK4W2::getSerial() const
{
#ifdef RTABMAP_K4W2
if (pKinectSensor_)
{
wchar_t uid[255] = { 0 };
// It seems to fail every time!?
HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
if (SUCCEEDED(hr))
{
std::wstring ws(uid);
return std::string(ws.begin(), ws.end());
}
}
#endif
return "";
}
@@ -2240,7 +2346,6 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
if (pDepthFrame && pColorFrame)
{
INT64 nDepthTime = 0;
IFrameDescription* pDepthFrameDescription = NULL;
int nDepthWidth = 0;
int nDepthHeight = 0;
@@ -2255,49 +2360,24 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
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)
@@ -2315,7 +2395,8 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
hr = E_FAIL;
}
}
if (SUCCEEDED(hr))
if(SUCCEEDED(hr))
{
//ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
// pColorBuffer, nColorWidth, nColorHeight,
@@ -2326,74 +2407,161 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
{
HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
if (SUCCEEDED(hr))
if (type_ == kTypeColor2DepthSD)
{
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)
HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
if (SUCCEEDED(hr))
{
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())
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)
{
// 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;
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);
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());
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());
}
else
{
UERROR("Failed color to depth registration!");
}
}
else //depthToColor
{
HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
if (SUCCEEDED(hr))
{
cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
cv::Mat depthRegistered = cv::Mat::zeros(
type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight,
type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
CV_16UC1);
cv::Mat imageColor;
if(type_ == kTypeDepth2ColorSD)
{
cv::Mat tmp;
cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
}
else
{
cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
}
// loop over output pixels
for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
{
ColorSpacePoint p = pColorCoordinates_[depthIndex];
// 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())
{
if (type_ == kTypeDepth2ColorSD)
{
p.X /= 2.0f;
p.Y /= 2.0f;
}
const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
// get the coordinate on image plane.
pixel_x_l = depthRegistered.cols - p.X; // flip depth
pixel_y_l = p.Y;
pixel_x_h = pixel_x_l - 1;
pixel_y_h = pixel_y_l + 1;
if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line
depth_value)
{
unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
if (depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
}
}
if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line
depth_value)
{
unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
if (depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
}
}
}
}
CameraModel model = colorCameraModel_;
if (type_ == kTypeDepth2ColorSD)
{
model = model.scaled(0.5);
}
util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
cv::flip(imageColor, imageColor, 1);
data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
}
else
{
UERROR("Failed depth to color registration!");
}
}
}
}