i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Freenect2: added new type Ir+Depth (RTAB-Map can be used without lightwith the Kinect v2)

Browse Source
This commit is contained in:
matlabbe
2015-05-05 19:32:09 -04:00
parent 679d9deca0
commit b5e62be664
3 changed files with 152 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraRGBD.h
+1
View File
@@ -278,6 +278,7 @@ public:
enum Type{ enum Type{
kTypeRGBDepthSD, kTypeRGBDepthSD,
kTypeRGBDepthHD, kTypeRGBDepthHD,
kTypeIRDepth,
kTypeRGBIR kTypeRGBIR
}; };
corelib/src/CameraRGBD.cpp
+144 -94
View File
@@ -1126,6 +1126,9 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const
case kTypeRGBIR: case kTypeRGBIR:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir); listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
break; break;
case kTypeIRDepth:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
break;
case kTypeRGBDepthSD: case kTypeRGBDepthSD:
case kTypeRGBDepthHD: case kTypeRGBDepthHD:
default: default:
@@ -1318,10 +1321,14 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
switch(type_) switch(type_)
{ {
case kTypeRGBIR: case kTypeRGBIR: //used for calibration
rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0); rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0); irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
break; break;
case kTypeIRDepth:
irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
break;
case kTypeRGBDepthSD: case kTypeRGBDepthSD:
case kTypeRGBDepthHD: case kTypeRGBDepthHD:
default: default:
@@ -1330,128 +1337,171 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
break; break;
} }
cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data); if(irFrame && depthFrame)
cv::flip(rgbMat, rgb, 1);
if(stereoModel_.isValid())
{ {
//rectify color cv::Mat irMat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data);
rgb = stereoModel_.right().rectifyImage(rgb); //convert to gray scaled
if(irFrame) float maxIr_ = 0x7FFF;
float minIr_ = 0x0;
const float factor = 255.0f / float((maxIr_ - minIr_));
rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
for(int i=0; i<irMat.rows; ++i)
{ {
//rectify IR for(int j=0; j<irMat.cols; ++j)
cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1); {
cv::flip(depth, depth, 1); rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
}
}
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(rgb, rgb, 1);
cv::flip(depth, depth, 1);
if(stereoModel_.isValid())
{
//rectify
rgb = stereoModel_.left().rectifyImage(rgb);
depth = stereoModel_.left().rectifyImage(depth); depth = stereoModel_.left().rectifyImage(depth);
fx = stereoModel_.left().fx();
fy = stereoModel_.left().fy();
cx = stereoModel_.left().cx();
cy = stereoModel_.left().cy();
} }
else else
{ {
//rectify depth libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); fx = params.fx;
cv::flip(depth, depth, 1); fy = params.fy;
depth = stereoModel_.left().rectifyDepth(depth); cx = params.cx;
cy = params.cy;
bool registered = true;
if(registered)
{
depth = util3d::registerDepth(
depth,
stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
stereoModel_.transform());
util3d::fillRegisteredDepthHoles(depth, true, false);
fx = stereoModel_.right().fx();
fy = stereoModel_.right().fy();
cx = stereoModel_.right().cx();
cy = stereoModel_.right().cy();
}
else
{
fx = stereoModel_.left().fx();
fy = stereoModel_.left().fy();
cx = stereoModel_.left().cx();
cy = stereoModel_.left().cy();
}
} }
} }
else else
{ {
//use data from libfreenect2 //rgb + ir or rgb + depth
if(irFrame)
cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
cv::flip(rgbMat, rgb, 1);
if(stereoModel_.isValid())
{ {
cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1); //rectify color
rgb = stereoModel_.right().rectifyImage(rgb);
if(irFrame)
{
//rectify IR
cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
depth = stereoModel_.left().rectifyImage(depth);
}
else
{
//rectify depth
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
depth = stereoModel_.left().rectifyDepth(depth);
bool registered = true;
if(registered)
{
depth = util3d::registerDepth(
depth,
stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
stereoModel_.transform());
util3d::fillRegisteredDepthHoles(depth, true, false);
fx = stereoModel_.right().fx();
fy = stereoModel_.right().fy();
cx = stereoModel_.right().cx();
cy = stereoModel_.right().cy();
}
else
{
fx = stereoModel_.left().fx();
fy = stereoModel_.left().fy();
cx = stereoModel_.left().cx();
cy = stereoModel_.left().cy();
}
}
} }
else else
{ {
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); //use data from libfreenect2
if(irFrame)
//registration of the depth
if(reg_)
{ {
if(type_ == kTypeRGBDepthSD) cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
}
else
{
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
//registration of the depth
if(reg_)
{ {
cv::Mat tmp; if(type_ == kTypeRGBDepthSD)
cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
rgb = tmp;
}
cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
{
for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
{ {
float dz = depthFrameMat.at<float>(dy,dx); cv::Mat tmp;
float dz1 = depthFrameMat.at<float>(dy,dx+1); cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
float dz2 = depthFrameMat.at<float>(dy+1,dx); rgb = tmp;
float dz3 = depthFrameMat.at<float>(dy+1,dx+1); }
if(dz && dz1 && dz2 && dz3) cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
{
for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
{ {
float avg = (dz + dz1 + dz2 + dz3) / 4; float dz = depthFrameMat.at<float>(dy,dx);
float thres = 0.01 * avg; float dz1 = depthFrameMat.at<float>(dy,dx+1);
if( fabs(dz - avg) < thres && float dz2 = depthFrameMat.at<float>(dy+1,dx);
fabs(dz1 - avg) < thres && float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
fabs(dz2 - avg) < thres && if(dz && dz1 && dz2 && dz3)
fabs(dz3 - avg) < thres)
{ {
float cx=-1,cy=-1; float avg = (dz + dz1 + dz2 + dz3) / 4;
reg_->apply(dx, dy, dz, cx, cy); float thres = 0.01 * avg;
if(type_==kTypeRGBDepthSD) if( fabs(dz - avg) < thres &&
fabs(dz1 - avg) < thres &&
fabs(dz2 - avg) < thres &&
fabs(dz3 - avg) < thres)
{ {
cx/=2.0f; float cx=-1,cy=-1;
cy/=2.0f; reg_->apply(dx, dy, dz, cx, cy);
} if(type_==kTypeRGBDepthSD)
int rcx = cvRound(cx);
int rcy = cvRound(cy);
if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
{
unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
if(zReg == 0 || zReg > (unsigned short)dz)
{ {
zReg = (unsigned short)dz; cx/=2.0f;
cy/=2.0f;
}
int rcx = cvRound(cx);
int rcy = cvRound(cy);
if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
{
unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
if(zReg == 0 || zReg > (unsigned short)dz)
{
zReg = (unsigned short)dz;
}
} }
} }
} }
} }
} }
util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
}
else
{
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
} }
util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
}
else
{
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
} }
cv::flip(depth, depth, 1);
} }
cv::flip(depth, depth, 1);
} }
listener_->release(frames); listener_->release(frames);
} }
guilib/src/ui/preferencesDialog.ui
+7 -2
View File
@@ -63,9 +63,9 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-485</y> <y>-591</y>
<width>760</width> <width>760</width>
<height>1132</height> <height>1502</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -1974,6 +1974,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>RGB+Depth HD</string> <string>RGB+Depth HD</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>IR+Depth</string>
</property>
</item>
</widget> </widget>
</item> </item>
</layout> </layout>