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

Updated CameraFreenect2 with latest freenect2 library (commit cdeb07917c)

Browse Source
This commit is contained in:
matlabbe
2015-11-03 17:31:07 -05:00
parent 620ce51f70
commit 2ee9183c17
11 changed files with 190 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -218,16 +218,17 @@ public:
static bool available();
enum Type{
kTypeRGBDepthSD,
kTypeRGBDepthHD,
kTypeColor2DepthSD,
kTypeDepth2ColorHD,
kTypeDepth2ColorSD,
kTypeIRDepth,
kTypeRGBIR
kTypeColorIR
};
public:
// default local transform z in, x right, y down));
CameraFreenect2(int deviceId= 0,
Type type = kTypeRGBDepthSD,
Type type = kTypeColor2DepthSD,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect2();
@@ -248,6 +249,8 @@ private:
libfreenect2::PacketPipeline * pipeline_;
libfreenect2::SyncMultiFrameListener * listener_;
libfreenect2::Registration * reg_;
double minKinect2Depth_;
double maxKinect2Depth_;
};

140
corelib/src/CameraRGBD.cpp
View File

@@ -1090,20 +1090,23 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const
dev_(0),
pipeline_(0),
listener_(0),
reg_(0)
reg_(0),
minKinect2Depth_(0.1),
maxKinect2Depth_(12.0)
{
#ifdef WITH_FREENECT2
freenect2_ = new libfreenect2::Freenect2();
switch(type_)
{
case kTypeRGBIR:
case kTypeColorIR:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
break;
case kTypeIRDepth:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
break;
case kTypeRGBDepthSD:
case kTypeRGBDepthHD:
case kTypeColor2DepthSD:
case kTypeDepth2ColorHD:
case kTypeDepth2ColorSD:
default:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
break;
@@ -1126,8 +1129,8 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const
libfreenect2::DepthPacketProcessor::Config config;
config.EnableBilateralFilter = true;
config.EnableEdgeAwareFilter = true;
config.MinDepth = 0.1;
config.MaxDepth = 12;
config.MinDepth = minKinect2Depth_;
config.MaxDepth = maxKinect2Depth_;
pipeline_->getDepthPacketProcessor()->setConfiguration(config);
#endif
@@ -1220,10 +1223,16 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
}
else
{
if(type_==kTypeColor2DepthSD)
{
UWARN("Freenect2: When using custom calibration file, type kTypeColor2DepthSD is not supported. kTypeDepth2ColorSD is used instead...");
type_ = kTypeDepth2ColorSD;
}
// downscale color image by 2
cv::Mat colorP = stereoModel_.right().P();
cv::Size colorSize = stereoModel_.right().imageSize();
if(type_ == kTypeRGBDepthSD)
if(type_ == kTypeDepth2ColorSD)
{
colorP.at<double>(0,0)/=2.0f; //fx
colorP.at<double>(1,1)/=2.0f; //fy
@@ -1306,7 +1315,7 @@ SensorData CameraFreenect2::captureImage()
switch(type_)
{
case kTypeRGBIR: //used for calibration
case kTypeColorIR: //used for calibration
rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
break;
@@ -1314,8 +1323,9 @@ SensorData CameraFreenect2::captureImage()
irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
break;
case kTypeRGBDepthSD:
case kTypeRGBDepthHD:
case kTypeColor2DepthSD:
case kTypeDepth2ColorHD:
case kTypeDepth2ColorSD:
default:
rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
@@ -1367,14 +1377,13 @@ SensorData CameraFreenect2::captureImage()
else
{
//rgb + ir or rgb + depth
if(stereoModel_.isValid())
{
cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
cv::Mat rgbMat; // rtabmap uses 3 channels RGB
cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
cv::flip(rgbMat, rgb, 1);
if(stereoModel_.isValid())
{
//rectify color
rgb = stereoModel_.right().rectifyImage(rgb);
if(irFrame)
@@ -1419,23 +1428,86 @@ SensorData CameraFreenect2::captureImage()
//use data from libfreenect2
if(irFrame)
{
cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
cv::Mat rgbMat; // rtabmap uses 3 channels RGB
cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
cv::flip(rgbMat, rgb, 1);
cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
}
else
{
cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
//registration of the depth
if(reg_)
UASSERT(reg_!=0);
if(type_ != kTypeDepth2ColorSD)
{
if(type_ == kTypeRGBDepthSD)
cv::Mat rgbMatBGRA;
libfreenect2::Frame depthUndistorted(512, 424, 4);
libfreenect2::Frame rgbRegistered(512, 424, 4);
libfreenect2::Frame bidDepth(1920, 1082, 4); // HD
reg_->apply(rgbFrame, depthFrame, &depthUndistorted, &rgbRegistered, true, &bidDepth);
if(type_ == kTypeColor2DepthSD)
{
rgbMatBGRA = cv::Mat((int)rgbRegistered.height, (int)rgbRegistered.width, CV_8UC4, rgbRegistered.data);
cv::Mat((int)depthUndistorted.height, (int)depthUndistorted.width, CV_32FC1, depthUndistorted.data).convertTo(depth, CV_16U, 1);
//use IR params
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
}
else
{
rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
cv::Mat((int)bidDepth.height, (int)bidDepth.width, CV_32FC1, bidDepth.data).convertTo(depth, CV_16U, 1);
depth = depth(cv::Range(1, 1081), cv::Range::all());
unsigned short maxDepth = (unsigned short)(maxKinect2Depth_*1000.0);
if(maxDepth < 65535)
{
int size = 1920*1080;
for(int i=0; i<size;++i)
{
if(((unsigned short *)depth.data)[i] > maxDepth)
{
((unsigned short *)depth.data)[i] = 0;
}
}
}
//use color params
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
}
// rtabmap uses 3 channels RGB
cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
cv::flip(rgb, rgb, 1);
cv::flip(depth, depth, 1);
}
else //register depth to color
{
UASSERT(type_ == kTypeDepth2ColorSD || type_ == kTypeDepth2ColorHD);
cv::Mat rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
if(type_ == kTypeDepth2ColorSD)
{
cv::Mat tmp;
cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
rgb = tmp;
cv::resize(rgbMatBGRA, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
rgbMatBGRA = tmp;
}
// rtabmap uses 3 channels RGB
cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
cv::flip(rgb, rgb, 1);
cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
depth = cv::Mat::zeros(rgbMatBGRA.rows, rgbMatBGRA.cols, CV_16U);
for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
{
for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
@@ -1455,7 +1527,7 @@ SensorData CameraFreenect2::captureImage()
{
float cx=-1,cy=-1;
reg_->apply(dx, dy, dz, cx, cy);
if(type_==kTypeRGBDepthSD)
if(type_ == kTypeDepth2ColorSD)
{
cx/=2.0f;
cy/=2.0f;
@@ -1474,24 +1546,16 @@ SensorData CameraFreenect2::captureImage()
}
}
}
util2d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
util2d::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;
}
}
util2d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeDepth2ColorHD);
util2d::fillRegisteredDepthHoles(depth, type_==kTypeDepth2ColorSD, type_==kTypeDepth2ColorHD);//second pass
cv::flip(depth, depth, 1);
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
fy = params.fy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
cx = params.cx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
cy = params.cy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
}
}
}
}

13
corelib/src/util3d.cpp
View File

@@ -216,7 +216,6 @@ pcl::PointXYZ projectDepthTo3D(
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
pcl::PointXYZ pt;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
float depth = util2d::getDepth(depthImage, x, y, smoothing, maxZError);
if(depth)
@@ -232,7 +231,7 @@ pcl::PointXYZ projectDepthTo3D(
}
else
{
pt.x = pt.y = pt.z = bad_point;
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
return pt;
}
@@ -323,11 +322,13 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
{
pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
bool invalidColor = false;
if(!mono)
{
pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
invalidColor = pt.b <= 5 && pt.g <= 5 && pt.r <= 5;
}
else
{
@@ -335,14 +336,22 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.b = v;
pt.g = v;
pt.r = v;
invalidColor = v <= 0;
}
if(invalidColor)
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
else
{
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
}
}
}
return cloud;
}

4
guilib/include/rtabmap/gui/CameraViewer.h
View File

@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QDialog>
#include <rtabmap/core/SensorData.h>
class QSpinBox;
namespace rtabmap {
class ImageView;
@@ -55,6 +57,8 @@ private:
ImageView* imageView_;
CloudViewer* cloudView_;
bool processingImages_;
QSpinBox * decimationSpin_;
int validDecimationValue_;
};
} /* namespace rtabmap */

50
guilib/src/CameraViewer.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/CameraEvent.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/gui/ImageView.h>
#include <rtabmap/gui/CloudViewer.h>
#include <rtabmap/utilite/UCv2Qt.h>
@@ -36,6 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtCore/QMetaType>
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QLabel>
#include <QSpinBox>
#include <QDialogButtonBox>
namespace rtabmap {
@@ -45,7 +48,8 @@ CameraViewer::CameraViewer(QWidget * parent) :
QDialog(parent),
imageView_(new ImageView(this)),
cloudView_(new CloudViewer(this)),
processingImages_(false)
processingImages_(false),
validDecimationValue_(1)
{
qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
@@ -56,15 +60,27 @@ CameraViewer::CameraViewer(QWidget * parent) :
layout->addWidget(imageView_,1);
layout->addWidget(cloudView_,1);
QLabel * decimationLabel = new QLabel("Decimation", this);
decimationSpin_ = new QSpinBox(this);
decimationSpin_->setMinimum(1);
decimationSpin_->setMaximum(16);
decimationSpin_->setValue(1);
QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
buttonBox->setStandardButtons(QDialogButtonBox::Close);
connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
QHBoxLayout * layout2 = new QHBoxLayout();
layout2->addWidget(decimationLabel);
layout2->addWidget(decimationSpin_);
layout2->addStretch(1);
layout2->addWidget(buttonBox);
QVBoxLayout * vlayout = new QVBoxLayout(this);
vlayout->setMargin(0);
vlayout->setSpacing(0);
vlayout->addLayout(layout, 1);
vlayout->addWidget(buttonBox);
vlayout->addLayout(layout2);
this->setLayout(vlayout);
}
@@ -76,26 +92,48 @@ CameraViewer::~CameraViewer()
void CameraViewer::showImage(const rtabmap::SensorData & data)
{
if(!data.imageRaw().empty() || !data.depthOrRightRaw().empty())
{
if(data.imageRaw().cols % decimationSpin_->value() == 0 &&
data.imageRaw().rows % decimationSpin_->value() == 0 &&
data.depthOrRightRaw().cols % decimationSpin_->value() == 0 &&
data.depthOrRightRaw().rows % decimationSpin_->value() == 0)
{
validDecimationValue_ = decimationSpin_->value();
}
else
{
UWARN("Decimation (%d) must be a denominator of the width and height of "
"the image (color=%d/%d depth=%d/%d). Using last valid decimation value (%d).",
decimationSpin_->value(),
data.imageRaw().cols,
data.imageRaw().rows,
data.depthOrRightRaw().cols,
data.depthOrRightRaw().rows,
validDecimationValue_);
}
}
processingImages_ = true;
if(!data.imageRaw().empty())
{
imageView_->setImage(uCvMat2QImage(data.imageRaw()));
imageView_->setImage(uCvMat2QImage(util2d::decimate(data.imageRaw(), validDecimationValue_)));
}
if(!data.depthOrRightRaw().empty())
{
imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw()));
imageView_->setImageDepth(uCvMat2QImage(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_)));
}
if((data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid())))
{
if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
{
cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data));
cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_));
cloudView_->setVisible(true);
cloudView_->update();
}
else if(!data.depthOrRightRaw().empty())
{
cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data));
cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_));
cloudView_->setVisible(true);
cloudView_->update();
}

2
guilib/src/PreferencesDialog.cpp
View File

@@ -3626,7 +3626,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
{
camera = new CameraFreenect2(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
useRawImages?CameraFreenect2::kTypeRGBIR:(CameraFreenect2::Type)_ui->comboBox_freenect2Format->currentIndex(),
useRawImages?CameraFreenect2::kTypeColorIR:(CameraFreenect2::Type)_ui->comboBox_freenect2Format->currentIndex(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}

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

@@ -64,8 +64,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>760</width>
<height>1705</height>
<width>759</width>
<height>966</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>16</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1874,7 +1874,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_rgbd">
<property name="currentIndex">
<number>4</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_32">
<layout class="QVBoxLayout" name="verticalLayout_63">
@@ -2145,17 +2145,22 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
<item>
<property name="text">
<string>RGB+Depth SD</string>
<string>Registered Color to Depth SD</string>
</property>
</item>
<item>
<property name="text">
<string>RGB+Depth HD</string>
<string>Registered Depth to Color HD</string>
</property>
</item>
<item>
<property name="text">
<string>IR+Depth</string>
<string>Registered Depth to Color SD</string>
</property>
</item>
<item>
<property name="text">
<string>IR + Depth</string>
</property>
</item>
</widget>

2
tools/Calibration/main.cpp
View File

@@ -184,7 +184,7 @@ int main(int argc, char * argv[])
exit(-1);
}
switchImages = true;
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBIR);
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColorIR);
}
else if(driver == 6)
{

2
tools/CameraRGBD/main.cpp
View File

@@ -214,7 +214,7 @@ int main(int argc, char * argv[])
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD);
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD);
}
}
else if(driver == 6)

2
tools/DataRecorder/main.cpp
View File

@@ -224,7 +224,7 @@ int main (int argc, char * argv[])
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(deviceId.size()?atoi(deviceId.c_str()):0, rtabmap::CameraFreenect2::kTypeRGBDepthSD, rate, t);
camera = new rtabmap::CameraFreenect2(deviceId.size()?atoi(deviceId.c_str()):0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
}
else if(driver == 6)
{

2
tools/OdometryViewer/main.cpp
View File

@@ -775,7 +775,7 @@ int main (int argc, char * argv[])
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD, rate, t);
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
}
else if(driver == 6)
{