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

With Freenect, added an error when no frames are received since 2 seconds (a bug that I have always at the first time I start the camera... then restarting the camera resolves the problem)

Browse Source
This commit is contained in:
matlabbe
2015-04-24 12:09:20 -04:00
parent fa3a2421f6
commit a16a2d65cf
1 changed files with 24 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
corelib/src/CameraRGBD.cpp
View File

@@ -861,13 +861,18 @@ class FreenectDevice : public UThread {
{
if(this->isRunning())
{
dataReady_.acquire();
UScopeMutex s(dataMutex_);
rgb = rgbLastFrame_;
depth = depthLastFrame_;
rgbLastFrame_ = cv::Mat();
depthLastFrame_= cv::Mat();
if(!dataReady_.acquire(1, 2000))
{
UERROR("Not received any frames since 2 seconds, try to restart the camera again.");
}
else
{
UScopeMutex s(dataMutex_);
rgb = rgbLastFrame_;
depth = depthLastFrame_;
rgbLastFrame_ = cv::Mat();
depthLastFrame_= cv::Mat();
}
}
}
@@ -1063,20 +1068,24 @@ std::string CameraFreenect::getSerial() const
void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
#ifdef WITH_FREENECT
rgb = cv::Mat();
depth = cv::Mat();
fx = 0.0f;
fy = 0.0f;
cx = 0.0f;
cy = 0.0f;
if(ctx_ && freenectDevice_)
{
if(freenectDevice_->isRunning())
{
freenectDevice_->getData(rgb, depth);
UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
fx = freenectDevice_->getDepthFocal();
fy = freenectDevice_->getDepthFocal();
cx = float(depth.cols/2) - 0.5f;
cy = float(depth.rows/2) - 0.5f;
if(depth.empty())
if(!rgb.empty() && !depth.empty())
{
UWARN("CameraFreenect: Data not ready! Try to reduce the image rate to avoid this warning...");
UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
fx = freenectDevice_->getDepthFocal();
fy = freenectDevice_->getDepthFocal();
cx = float(depth.cols/2) - 0.5f;
cy = float(depth.rows/2) - 0.5f;
}
}
else
@@ -1086,15 +1095,6 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl
freenectDevice_ = 0;
}
}
if(depth.empty() || rgb.empty())
{
rgb = cv::Mat();
depth = cv::Mat();
fx = 0.0f;
fy = 0.0f;
cx = 0.0f;
cy = 0.0f;
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif