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

Refactoring CameraRGBD class (now including OpenNI, OpenNI2, OpenNI from OpenCV and Freenect)

Browse Source 
Added tool to test RGB-D camera: rtabmap-rgbd_camera
Fixed Freenect corrupted depth image (after some time)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1366 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-15 03:31:35 +00:00
parent 81b3b49f97
commit c18f1501d4
25 changed files with 1248 additions and 1579 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/CMakeLists.txt
View File

@@ -13,8 +13,7 @@ SET(SRC_FILES
Camera.cpp
CameraThread.cpp
CameraOpenni.cpp
CameraFreenect.cpp
CameraRGBD.cpp
EpipolarGeometry.cpp
VisualWord.cpp

405
corelib/src/Camera.cpp
View File

@@ -33,23 +33,15 @@
#include <iostream>
#include <cmath>
#ifdef WITH_OPENNI2
#endif
#include <OpenNI.h>
namespace rtabmap
{
Camera::Camera(float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped) :
unsigned int imageHeight) :
_imageRate(imageRate),
_imageWidth(imageWidth),
_imageHeight(imageHeight),
_framesDropped(framesDropped),
_localTransform(Transform::getIdentity()),
_frameRateTimer(new UTimer())
{
}
@@ -76,21 +68,7 @@ void Camera::getImageSize(unsigned int & width, unsigned int & height)
cv::Mat Camera::takeImage()
{
cv::Mat rgb, depth;
float depthConstant = 0.0f;
takeImage(rgb, depth, depthConstant);
return rgb;
}
void Camera::takeImage(cv::Mat & rgb)
{
cv::Mat depth;
float depthConstant = 0.0f;
takeImage(rgb, depth, depthConstant);
}
void Camera::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
cv::Mat img;
float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
if(imageRate>0)
{
@@ -112,33 +90,9 @@ void Camera::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
}
UTimer timer;
this->captureImage(rgb, depth, depthConstant);
img = this->captureImage();
UDEBUG("Time capturing image = %fs", timer.ticks());
if(!rgb.empty())
{
UASSERT(rgb.depth() == CV_8U);
if(_framesDropped)
{
unsigned int count = 0;
while(count++ < _framesDropped)
{
cv::Mat tmp,tmp2;
float tmpf;
this->captureImage(tmp, tmp2, tmpf);
if(!tmp.empty())
{
UDEBUG("frame dropped (%d/%d)", (int)count, (int)_framesDropped);
}
else
{
break;
}
}
UDEBUG("Frames dropped time = %fs", timer.ticks());
}
}
return img;
}
/////////////////////////
@@ -149,9 +103,8 @@ CameraImages::CameraImages(const std::string & path,
bool refreshDir,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped) :
Camera(imageRate, imageWidth, imageHeight, framesDropped),
unsigned int imageHeight) :
Camera(imageRate, imageWidth, imageHeight),
_path(path),
_startAt(startAt),
_refreshDir(refreshDir),
@@ -196,8 +149,9 @@ bool CameraImages::init()
return _dir->isValid();
}
void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
cv::Mat CameraImages::captureImage()
{
cv::Mat img;
UDEBUG("");
if(_dir->isValid())
{
@@ -214,7 +168,7 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
{
_lastFileName = *fileNames.rbegin();
std::string fullPath = _path + _lastFileName;
rgb = cv::imread(fullPath.c_str());
img = cv::imread(fullPath.c_str());
}
}
}
@@ -234,19 +188,20 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
{
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
rgb = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
#else
rgb = cv::imread(fullPath.c_str(), -1);
img = cv::imread(fullPath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", rgb.cols, rgb.rows, rgb.channels(), rgb.elemSize(), rgb.total());
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
img.cols, img.rows, img.channels(), img.elemSize(), img.total());
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(rgb.depth() != CV_8U)
if(img.depth() != CV_8U)
{
// The depth should be 8U
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
IplImage * i = cvLoadImage(fullPath.c_str());
rgb = cv::Mat(i, true);
img = cv::Mat(i, true);
cvReleaseImage(&i);
}
}
@@ -262,16 +217,17 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
unsigned int h;
this->getImageSize(w, h);
if(!rgb.empty() &&
if(!img.empty() &&
w &&
h &&
w != (unsigned int)rgb.cols &&
h != (unsigned int)rgb.rows)
w != (unsigned int)img.cols &&
h != (unsigned int)img.rows)
{
cv::Mat resampled;
cv::resize(rgb, resampled, cv::Size(w, h));
rgb = resampled;
cv::resize(img, resampled, cv::Size(w, h));
img = resampled;
}
return img;
}
@@ -282,9 +238,8 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
CameraVideo::CameraVideo(int usbDevice,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped) :
Camera(imageRate, imageWidth, imageHeight, framesDropped),
unsigned int imageHeight) :
Camera(imageRate, imageWidth, imageHeight),
_src(kUsbDevice),
_usbDevice(usbDevice)
{
@@ -294,9 +249,8 @@ CameraVideo::CameraVideo(int usbDevice,
CameraVideo::CameraVideo(const std::string & filePath,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped) :
Camera(imageRate, imageWidth, imageHeight, framesDropped),
unsigned int imageHeight) :
Camera(imageRate, imageWidth, imageHeight),
_filePath(filePath),
_src(kVideoFile),
_usbDevice(0)
@@ -348,30 +302,31 @@ bool CameraVideo::init()
return true;
}
void CameraVideo::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
cv::Mat CameraVideo::captureImage()
{
cv::Mat img;
if(_capture.isOpened())
{
if(_capture.read(rgb))
if(_capture.read(img))
{
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
if(!rgb.empty() &&
if(!img.empty() &&
w &&
h &&
w != (unsigned int)rgb.cols &&
h != (unsigned int)rgb.rows)
w != (unsigned int)img.cols &&
h != (unsigned int)img.rows)
{
cv::Mat resampled;
cv::resize(rgb, resampled, cv::Size(w, h));
rgb = resampled;
cv::resize(img, resampled, cv::Size(w, h));
img = resampled;
}
else
{
// clone required
rgb = rgb.clone();
img = img.clone();
}
}
else if(_usbDevice)
@@ -383,297 +338,7 @@ void CameraVideo::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCons
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
}
/////////////////////////
// CameraRGBD
/////////////////////////
bool CameraRGBD::available()
{
return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
}
CameraRGBD::CameraRGBD(float imageRate, bool asus) :
Camera(imageRate),
_asus(asus),
_depthFocal(0.0f)
{
}
CameraRGBD::~CameraRGBD()
{
_capture.release();
}
bool CameraRGBD::init()
{
if(_capture.isOpened())
{
_capture.release();
}
ULOGGER_DEBUG("CameraRGBD::init()");
_capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
if(_capture.isOpened())
{
_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
_depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
// Print some avalible device settings.
UINFO("Depth generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
{
UERROR("Depth registration is not activated on this device!");
}
if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
{
UINFO("Image generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
}
else
{
UERROR("CameraRGBD: Device doesn't contain image generator.");
_capture.release();
return false;
}
}
else
{
ULOGGER_ERROR("CameraRGBD: Failed to create a capture object!");
_capture.release();
return false;
}
return true;
}
void CameraRGBD::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
if(_capture.isOpened())
{
_capture.grab();
_capture.retrieve( depth, CV_CAP_OPENNI_DEPTH_MAP );
_capture.retrieve( rgb, CV_CAP_OPENNI_BGR_IMAGE );
depth = depth.clone();
rgb = rgb.clone();
UASSERT(_depthFocal > 0.0f);
depthConstant = 1.0f/_depthFocal;
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
}
/////////////////////////
// CameraOpenNI2
/////////////////////////
bool CameraOpenNI2::available()
{
#ifdef WITH_OPENNI2
return true;
#else
return false;
#endif
}
CameraOpenNI2::CameraOpenNI2(float imageRate) :
Camera(imageRate),
_device(new openni::Device()),
_color(new openni::VideoStream()),
_depth(new openni::VideoStream()),
_depthFocal(0.0f)
{
}
CameraOpenNI2::~CameraOpenNI2()
{
_color->stop();
_color->destroy();
_depth->stop();
_depth->destroy();
_device->close();
openni::OpenNI::shutdown();
delete _device;
delete _color;
delete _depth;
}
bool CameraOpenNI2::init()
{
openni::OpenNI::initialize();
if(_device->open(openni::ANY_DEVICE) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot open device.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
{
UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
_device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
{
UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot create depth stream.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot create color stream.");
_depth->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Failed to set depth/color registration.");
}
_depth->setMirroringEnabled(false);
_color->setMirroringEnabled(false);
const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
for(int i=0; i<depthVideoModes.getSize(); ++i)
{
UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
i,
depthVideoModes[i].getFps(),
depthVideoModes[i].getPixelFormat(),
depthVideoModes[i].getResolutionX(),
depthVideoModes[i].getResolutionY());
}
const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
for(int i=0; i<colorVideoModes.getSize(); ++i)
{
UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
i,
colorVideoModes[i].getFps(),
colorVideoModes[i].getPixelFormat(),
colorVideoModes[i].getResolutionX(),
colorVideoModes[i].getResolutionY());
}
openni::VideoMode mMode;
mMode.setFps(30);
mMode.setResolution(640,480);
mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
_depth->setVideoMode(mMode);
openni::VideoMode mModeColor;
mModeColor.setFps(30);
mModeColor.setResolution(640,480);
mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
_color->setVideoMode(mModeColor);
UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
_depth->getVideoMode().getFps(),
_depth->getVideoMode().getPixelFormat(),
_depth->getVideoMode().getResolutionX(),
_depth->getVideoMode().getResolutionY(),
_depth->getHorizontalFieldOfView(),
_depth->getVerticalFieldOfView());
bool registered = true;
if(registered)
{
_depthFocal = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
}
else
{
_depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
}
UINFO("depth focal = %f", _depthFocal);
UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
_color->getVideoMode().getFps(),
_color->getVideoMode().getPixelFormat(),
_color->getVideoMode().getResolutionX(),
_color->getVideoMode().getResolutionY(),
_color->getHorizontalFieldOfView(),
_color->getVerticalFieldOfView());
if(_depth->start() != openni::STATUS_OK ||
_color->start() != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
_depth->stop();
_color->stop();
_depth->destroy();
_color->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
uSleep(1000); // just to make sure the sensor is correctly initialized
return true;
}
void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
if(_device->isValid() &&
_depth->isValid() &&
_color->isValid() &&
_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
_device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
{
openni::VideoFrameRef depthFrame, colorFrame;
_depth->readFrame(&depthFrame);
_color->readFrame(&colorFrame);
if(depthFrame.isValid() && colorFrame.isValid())
{
int h=depthFrame.getHeight();
int w=depthFrame.getWidth();
depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
h=colorFrame.getHeight();
w=colorFrame.getWidth();
cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
cv::cvtColor(tmp, rgb, CV_RGB2BGR);
}
UASSERT(_depthFocal != 0.0f);
depthConstant = 1.0f/_depthFocal;
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
return img;
}
} // namespace rtabmap

316
corelib/src/CameraFreenect.cpp
View File

@@ -1,316 +0,0 @@
/*
* CameraFreenect.cpp
*
* Created on: 2014-06-02
* Author: Mathieu
*/
#include "rtabmap/core/CameraFreenect.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/utilite/ULogger.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <opencv2/imgproc/imgproc.hpp>
#ifdef WITH_FREENECT
#include <libfreenect.h>
#include <libfreenect-registration.h>
#endif
namespace rtabmap {
//
// FreenectDevice
//
FreenectDevice::FreenectDevice(freenect_context * ctx, int index) :
index_(index),
ctx_(ctx),
device_(0),
depthMat_(cv::Size(640,480),CV_16UC1),
rgbMat_(cv::Size(640,480), CV_8UC3, cv::Scalar(0)),
depthReady_(false),
rgbReady_(false),
depthFocal_(0.0f)
{
UASSERT(ctx_ != 0);
}
#ifdef WITH_FREENECT
FreenectDevice::~FreenectDevice() {
if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
}
void FreenectDevice::startVideo() {
if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
}
void FreenectDevice::stopVideo() {
if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
}
void FreenectDevice::startDepth() {
if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
}
void FreenectDevice::stopDepth() {
if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
}
bool FreenectDevice::init()
{
if(freenect_open_device(ctx_, &device_, index_) < 0)
{
UERROR("FreenectDevice: Cannot open Kinect");
return false;
}
freenect_set_user(device_, this);
freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
freenect_set_depth_callback(device_, freenect_depth_callback);
freenect_set_video_callback(device_, freenect_video_callback);
bool registered = true;
float rgb_focal_length_sxga = 1050.0f;
float width_sxga = 1280.0f;
float width = freenect_get_current_depth_mode(device_).width;
float scale = width / width_sxga;
if(registered)
{
depthFocal_ = rgb_focal_length_sxga * scale;
}
else
{
freenect_registration reg = freenect_copy_registration(device_);
float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
freenect_destroy_registration(&reg);
depthFocal_ = depth_focal_length_sxga * scale;
}
UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
return true;
}
void FreenectDevice::freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
device->DepthCallback(depth, timestamp);
}
void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
device->VideoCallback(video, timestamp);
}
#else
FreenectDevice::~FreenectDevice() {}
void FreenectDevice::startVideo() {}
void FreenectDevice::stopVideo() {}
void FreenectDevice::startDepth() {}
void FreenectDevice::stopDepth() {}
bool FreenectDevice::init()
{
UERROR("RTAB-Map is not built with Freenect support!");
return false;
}
void FreenectDevice::freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {}
void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {}
#endif
// Do not call directly even in child
void FreenectDevice::VideoCallback(void* _rgb, uint32_t timestamp)
{
rgbMutex_.lock();
uint8_t* rgb = static_cast<uint8_t*>(_rgb);
rgbMat_.data = rgb;
rgbReady_ = true;
rgbMutex_.unlock();
}
// Do not call directly even in child
void FreenectDevice::DepthCallback(void* _depth, uint32_t timestamp)
{
depthMutex_.lock();
uint16_t* depth = static_cast<uint16_t*>(_depth);
depthMat_.data = (uchar*) depth;
depthReady_ = true;
depthMutex_.unlock();
}
cv::Mat FreenectDevice::getRgb()
{
cv::Mat out;
rgbMutex_.lock();
if(rgbReady_)
{
cv::cvtColor(rgbMat_, out, CV_RGB2BGR);
rgbReady_ = false;
}
rgbMutex_.unlock();
return out;
}
cv::Mat FreenectDevice::getDepth()
{
cv::Mat out;
depthMutex_.lock();
if(depthReady_)
{
depthMat_.copyTo(out);
depthReady_ = false;
}
depthMutex_.unlock();
return out;
}
//
// CameraFreenect
//
bool CameraFreenect::available()
{
#ifdef WITH_FREENECT
return true;
#else
return false;
#endif
}
CameraFreenect::CameraFreenect(int deviceId, float inputRate, const Transform & localTransform) :
deviceId_(deviceId),
rate_(inputRate),
frameRateTimer_(new UTimer()),
localTransform_(localTransform),
seq_(0),
ctx_(0),
freenectDevice_(0)
{
#ifdef WITH_FREENECT
if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
// We claim both the motor and camera devices, since this class exposes both.
// It does not support audio, so we do not claim it.
freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
#endif
}
CameraFreenect::~CameraFreenect()
{
UDEBUG("");
join(true);
if(freenectDevice_)
{
delete freenectDevice_;
freenectDevice_ = 0;
}
#ifdef WITH_FREENECT
if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
#endif
delete frameRateTimer_;
}
bool CameraFreenect::init()
{
#ifdef WITH_FREENECT
if(!this->isRunning())
{
if(freenectDevice_)
{
delete freenectDevice_;
freenectDevice_ = 0;
}
seq_ = 0;
if(freenect_num_devices(ctx_) > 0)
{
freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
if(freenectDevice_->init())
{
return true;
}
delete freenectDevice_;
freenectDevice_ = 0;
}
else
{
UERROR("CameraFreenect: No devices connected!");
}
}
else
{
UERROR("CameraFreenect: Cannot initialize the camera because it is already running...");
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif
return false;
}
void CameraFreenect::setFrameRate(float rate)
{
rate_ = rate;
}
void CameraFreenect::mainLoopBegin()
{
if(freenectDevice_)
{
freenectDevice_->startDepth();
freenectDevice_->startVideo();
frameRateTimer_->start();
}
else
{
UERROR("CameraFreenect: init should be called before starting the camera.");
this->kill();
}
}
void CameraFreenect::mainLoop()
{
#ifdef WITH_FREENECT
timeval t;
t.tv_sec = 0;
t.tv_usec = 10000;
if(freenect_process_events_timeout(ctx_, &t) < 0) UERROR("Cannot process freenect events");
if(freenectDevice_ && !this->isKilled())
{
float imageRate = rate_==0.0f?33.0f:rate_; // limit to 33Hz if infinity
if(frameRateTimer_->getElapsedTime() >= 1.0/double(imageRate)-0.000001)
{
double slept = frameRateTimer_->getElapsedTime();
frameRateTimer_->start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
cv::Mat depth = freenectDevice_->getDepth();
cv::Mat rgb = freenectDevice_->getRgb();
if(depth.empty())
{
UWARN("CameraFreenect: Depth not ready! Try to reduce the image rate to avoid this warning...");
return;
}
if(rgb.empty())
{
UWARN("CameraFreenect: Rgb not ready! Try to reduce the image rate to avoid this warning...");
return;
}
UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
float constant = 1.0f/freenectDevice_->getDepthFocal();
this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_));
}
}
#endif
}
void CameraFreenect::mainLoopEnd()
{
if(freenectDevice_)
{
freenectDevice_->stopDepth();
freenectDevice_->stopVideo();
}
}
} /* namespace rtabmap */

152
corelib/src/CameraOpenni.cpp
View File

@@ -1,152 +0,0 @@
/*
* CameraOpenni.cpp
*
* Created on: 2013-08-22
* Author: Mathieu
*/
#include "rtabmap/core/CameraOpenni.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/utilite/ULogger.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <pcl/io/openni_grabber.h>
namespace rtabmap {
CameraOpenni::CameraOpenni(const std::string & deviceId, float inputRate, const Transform & localTransform) :
interface_(0),
deviceId_(deviceId),
rate_(inputRate),
frameRateTimer_(new UTimer()),
localTransform_(localTransform),
seq_(0)
{
}
CameraOpenni::~CameraOpenni()
{
UDEBUG("");
kill();
delete frameRateTimer_;
if(interface_)
{
uSleep(100); // make sure it is stopped
delete interface_;
interface_ = 0;
}
}
void CameraOpenni::image_cb (
const boost::shared_ptr<openni_wrapper::Image>& rgb,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant)
{
if(rate_>0.0f)
{
if(frameRateTimer_->getElapsedTime() < 1.0f/rate_)
{
return;
}
}
frameRateTimer_->start();
UTimer t;
cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
cv::Mat bgrFrame;
cv::cvtColor(rgbFrame, bgrFrame, CV_RGB2BGR);
cv::Mat depthFrame(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depthFrame.data);
UINFO("constant=%f focal=%f", constant, 1.0f/constant);
this->post(new CameraEvent(bgrFrame, depthFrame, constant, localTransform_, ++seq_));
}
bool CameraOpenni::init()
{
if(interface_ && interface_->isRunning())
{
UERROR("Already started!!!\n");
return false;
}
else if(interface_)
{
delete interface_;
interface_ = 0;
}
seq_ = 0;
try
{
interface_ = new pcl::OpenNIGrabber(deviceId_);
}
catch(const pcl::IOException& ex)
{
UERROR("OpenNI exception: %s", ex.what());
if(interface_)
{
delete interface_;
interface_ = 0;
}
return false;
}
frameRateTimer_->start();
return true;
}
void CameraOpenni::start()
{
if(interface_)
{
if(!connection_.connected())
{
boost::function<void (
const boost::shared_ptr<openni_wrapper::Image>&,
const boost::shared_ptr<openni_wrapper::DepthImage>&,
float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
connection_ = interface_->registerCallback (f);
}
if(!interface_->isRunning())
{
interface_->start ();
}
}
}
void CameraOpenni::pause()
{
if(connection_.connected())
{
connection_.disconnect();
}
}
void CameraOpenni::kill()
{
UDEBUG("");
if(interface_)
{
interface_->stop();
}
}
bool CameraOpenni::isRunning()
{
return (interface_ && interface_->isRunning());
}
void CameraOpenni::setFrameRate(float rate)
{
rate_ = rate;
}
} /* namespace rtabmap */

791
corelib/src/CameraRGBD.cpp Normal file
View File

@@ -0,0 +1,791 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/DBDriver.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UTimer.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <pcl/io/openni_grabber.h>
#include <cmath>
#ifdef WITH_FREENECT
#include <libfreenect.h>
#include <libfreenect-registration.h>
#endif
#ifdef WITH_OPENNI2
#include <OpenNI.h>
#endif
namespace rtabmap
{
CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform) :
_imageRate(imageRate),
_localTransform(localTransform),
_frameRateTimer(new UTimer())
{
}
CameraRGBD::~CameraRGBD()
{
if(_frameRateTimer)
{
delete _frameRateTimer;
}
}
void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
if(imageRate>0)
{
int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
{
//
}
double slept = _frameRateTimer->getElapsedTime();
_frameRateTimer->start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
}
UTimer timer;
this->captureImage(rgb, depth, depthConstant);
UDEBUG("Time capturing image = %fs", timer.ticks());
}
/////////////////////////
// CameraOpenNIPCL
/////////////////////////
CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
interface_(0),
deviceId_(deviceId)
{
}
CameraOpenni::~CameraOpenni()
{
UDEBUG("");
if(connection_.connected())
{
connection_.disconnect();
}
if(interface_)
{
interface_->stop();
uSleep(1000); // make sure it is stopped
delete interface_;
interface_ = 0;
}
}
void CameraOpenni::image_cb (
const boost::shared_ptr<openni_wrapper::Image>& rgb,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant)
{
UScopeMutex s(dataMutex_);
cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
depthConstant_ = constant;
if(dataReady_.value() <= 0)
{
dataReady_.release();
}
}
bool CameraOpenni::init()
{
if(interface_)
{
interface_->stop();
uSleep(100); // make sure it is stopped
delete interface_;
interface_ = 0;
}
try
{
interface_ = new pcl::OpenNIGrabber(deviceId_);
boost::function<void (
const boost::shared_ptr<openni_wrapper::Image>&,
const boost::shared_ptr<openni_wrapper::DepthImage>&,
float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
connection_ = interface_->registerCallback (f);
interface_->start ();
}
catch(const pcl::IOException& ex)
{
UERROR("OpenNI exception: %s", ex.what());
if(interface_)
{
delete interface_;
interface_ = 0;
}
return false;
}
return true;
}
void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
if(interface_ && interface_->isRunning())
{
dataReady_.acquire();
UScopeMutex s(dataMutex_);
depth = depth_;
rgb = rgb_;
depthConstant = depthConstant_;
depth_ = cv::Mat();
rgb_ = cv::Mat();
depthConstant_ = 0;
}
}
/////////////////////////
// CameraOpenNICV
/////////////////////////
bool CameraOpenNICV::available()
{
return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
}
CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
_asus(asus),
_depthFocal(0.0f)
{
}
CameraOpenNICV::~CameraOpenNICV()
{
_capture.release();
}
bool CameraOpenNICV::init()
{
if(_capture.isOpened())
{
_capture.release();
}
ULOGGER_DEBUG("CameraRGBD::init()");
_capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
if(_capture.isOpened())
{
_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
_depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
// Print some avalible device settings.
UINFO("Depth generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
{
UERROR("Depth registration is not activated on this device!");
}
if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
{
UINFO("Image generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
}
else
{
UERROR("CameraRGBD: Device doesn't contain image generator.");
_capture.release();
return false;
}
}
else
{
ULOGGER_ERROR("CameraRGBD: Failed to create a capture object!");
_capture.release();
return false;
}
return true;
}
void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
if(_capture.isOpened())
{
_capture.grab();
_capture.retrieve( depth, CV_CAP_OPENNI_DEPTH_MAP );
_capture.retrieve( rgb, CV_CAP_OPENNI_BGR_IMAGE );
depth = depth.clone();
rgb = rgb.clone();
UASSERT(_depthFocal > 0.0f);
depthConstant = 1.0f/_depthFocal;
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
}
/////////////////////////
// CameraOpenNI2
/////////////////////////
bool CameraOpenNI2::available()
{
#ifdef WITH_OPENNI2
return true;
#else
return false;
#endif
}
CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
#ifdef WITH_OPENNI2
_device(new openni::Device()),
_color(new openni::VideoStream()),
_depth(new openni::VideoStream()),
#else
_device(0),
_color(0),
_depth(0),
#endif
_depthFocal(0.0f)
{
}
CameraOpenNI2::~CameraOpenNI2()
{
#ifdef WITH_OPENNI2
_color->stop();
_color->destroy();
_depth->stop();
_depth->destroy();
_device->close();
openni::OpenNI::shutdown();
delete _device;
delete _color;
delete _depth;
#endif
}
bool CameraOpenNI2::init()
{
#ifdef WITH_OPENNI2
openni::OpenNI::initialize();
if(_device->open(openni::ANY_DEVICE) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot open device.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
{
UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
_device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
{
UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot create depth stream.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot create color stream.");
_depth->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Failed to set depth/color registration.");
}
_depth->setMirroringEnabled(false);
_color->setMirroringEnabled(false);
const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
for(int i=0; i<depthVideoModes.getSize(); ++i)
{
UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
i,
depthVideoModes[i].getFps(),
depthVideoModes[i].getPixelFormat(),
depthVideoModes[i].getResolutionX(),
depthVideoModes[i].getResolutionY());
}
const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
for(int i=0; i<colorVideoModes.getSize(); ++i)
{
UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
i,
colorVideoModes[i].getFps(),
colorVideoModes[i].getPixelFormat(),
colorVideoModes[i].getResolutionX(),
colorVideoModes[i].getResolutionY());
}
openni::VideoMode mMode;
mMode.setFps(30);
mMode.setResolution(640,480);
mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
_depth->setVideoMode(mMode);
openni::VideoMode mModeColor;
mModeColor.setFps(30);
mModeColor.setResolution(640,480);
mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
_color->setVideoMode(mModeColor);
UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
_depth->getVideoMode().getFps(),
_depth->getVideoMode().getPixelFormat(),
_depth->getVideoMode().getResolutionX(),
_depth->getVideoMode().getResolutionY(),
_depth->getHorizontalFieldOfView(),
_depth->getVerticalFieldOfView());
bool registered = true;
if(registered)
{
_depthFocal = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
}
else
{
_depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
}
UINFO("depth focal = %f", _depthFocal);
UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
_color->getVideoMode().getFps(),
_color->getVideoMode().getPixelFormat(),
_color->getVideoMode().getResolutionX(),
_color->getVideoMode().getResolutionY(),
_color->getHorizontalFieldOfView(),
_color->getVerticalFieldOfView());
if(_depth->start() != openni::STATUS_OK ||
_color->start() != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
_depth->stop();
_color->stop();
_depth->destroy();
_color->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
uSleep(1000); // just to make sure the sensor is correctly initialized
return true;
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
return false;
#endif
}
void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
#ifdef WITH_OPENNI2
if(_device->isValid() &&
_depth->isValid() &&
_color->isValid() &&
_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
_device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
{
openni::VideoFrameRef depthFrame, colorFrame;
_depth->readFrame(&depthFrame);
_color->readFrame(&colorFrame);
if(depthFrame.isValid() && colorFrame.isValid())
{
int h=depthFrame.getHeight();
int w=depthFrame.getWidth();
depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
h=colorFrame.getHeight();
w=colorFrame.getWidth();
cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
cv::cvtColor(tmp, rgb, CV_RGB2BGR);
}
UASSERT(_depthFocal != 0.0f);
depthConstant = 1.0f/_depthFocal;
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
#endif
}
#ifdef WITH_FREENECT
//
// FreenectDevice
//
class FreenectDevice : public UThread {
public:
FreenectDevice(freenect_context * ctx, int index) :
index_(index),
ctx_(ctx),
device_(0),
depthFocal_(0.0f)
{
UASSERT(ctx_ != 0);
}
~FreenectDevice()
{
this->join(true);
if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
}
bool init()
{
if(freenect_open_device(ctx_, &device_, index_) < 0)
{
UERROR("FreenectDevice: Cannot open Kinect");
return false;
}
freenect_set_user(device_, this);
freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
depthBuffer_ = cv::Mat(cv::Size(640,480),CV_16UC1);
rgbBuffer_ = cv::Mat(cv::Size(640,480), CV_8UC3);
freenect_set_depth_buffer(device_, depthBuffer_.data);
freenect_set_video_buffer(device_, rgbBuffer_.data);
freenect_set_depth_callback(device_, freenect_depth_callback);
freenect_set_video_callback(device_, freenect_video_callback);
bool registered = true;
float rgb_focal_length_sxga = 1050.0f;
float width_sxga = 1280.0f;
float width = freenect_get_current_depth_mode(device_).width;
float scale = width / width_sxga;
if(registered)
{
depthFocal_ = rgb_focal_length_sxga * scale;
}
else
{
freenect_registration reg = freenect_copy_registration(device_);
float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
freenect_destroy_registration(&reg);
depthFocal_ = depth_focal_length_sxga * scale;
}
UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
return true;
}
float getDepthFocal() const {return depthFocal_;}
void getData(cv::Mat & rgb, cv::Mat & depth)
{
if(this->isRunning())
{
dataReady_.acquire();
{
UScopeMutex s1(rgbMutex_);
if(!rgbLastFrame_.empty())
{
rgb = rgbLastFrame_;
rgbLastFrame_ = cv::Mat();
}
}
{
UScopeMutex s2(depthMutex_);
if(!depthLastFrame_.empty())
{
depth = depthLastFrame_;
depthLastFrame_= cv::Mat();
}
}
}
}
private:
// Do not call directly even in child
void VideoCallback(void* rgb)
{
UASSERT(rgbBuffer_.data == rgb);
UScopeMutex s1(rgbMutex_);
cv::cvtColor(rgbBuffer_, rgbLastFrame_, CV_RGB2BGR);
if(!depthLastFrame_.empty() && dataReady_.value() <= 0)
{
dataReady_.release();
}
}
// Do not call directly even in child
void DepthCallback(void* depth)
{
UASSERT(depthBuffer_.data == depth);
UScopeMutex s2(depthMutex_);
depthLastFrame_ = depthBuffer_.clone();
if(!rgbLastFrame_.empty() && dataReady_.value() <= 0)
{
dataReady_.release();
}
}
void startVideo() {
if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
}
void stopVideo() {
if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
}
void startDepth() {
if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
}
void stopDepth() {
if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
}
virtual void mainLoopBegin()
{
this->startDepth();
this->startVideo();
}
virtual void mainLoop()
{
timeval t;
t.tv_sec = 0;
t.tv_usec = 10000;
if(freenect_process_events_timeout(ctx_, &t) < 0)
{
UERROR("FreenectDevice: Cannot process freenect events");
this->kill();
}
}
virtual void mainLoopEnd()
{
this->stopDepth();
this->stopVideo();
dataReady_.release();
}
static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
device->DepthCallback(depth);
}
static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
device->VideoCallback(video);
}
//noncopyable
FreenectDevice( const FreenectDevice& );
const FreenectDevice& operator=( const FreenectDevice& );
private:
int index_;
freenect_context * ctx_;
freenect_device * device_;
cv::Mat depthBuffer_;
cv::Mat rgbBuffer_;
UMutex depthMutex_;
UMutex rgbMutex_;
cv::Mat depthLastFrame_;
cv::Mat rgbLastFrame_;
float depthFocal_;
USemaphore dataReady_;
};
#endif
//
// CameraFreenect
//
bool CameraFreenect::available()
{
#ifdef WITH_FREENECT
return true;
#else
return false;
#endif
}
CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
deviceId_(deviceId),
ctx_(0),
freenectDevice_(0)
{
#ifdef WITH_FREENECT
if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
// We claim both the motor and camera devices, since this class exposes both.
// It does not support audio, so we do not claim it.
freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
#endif
}
CameraFreenect::~CameraFreenect()
{
#ifdef WITH_FREENECT
if(freenectDevice_)
{
freenectDevice_->join(true);
delete freenectDevice_;
freenectDevice_ = 0;
}
if(ctx_)
{
if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
}
#endif
}
bool CameraFreenect::init()
{
#ifdef WITH_FREENECT
if(freenectDevice_)
{
freenectDevice_->join(true);
delete freenectDevice_;
freenectDevice_ = 0;
}
if(ctx_ && freenect_num_devices(ctx_) > 0)
{
freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
if(freenectDevice_->init())
{
freenectDevice_->start();
uSleep(3000);
return true;
}
else
{
UERROR("CameraFreenect: Init failed!");
}
delete freenectDevice_;
freenectDevice_ = 0;
}
else
{
UERROR("CameraFreenect: No devices connected!");
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif
return false;
}
void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
#ifdef WITH_FREENECT
if(ctx_ && freenectDevice_)
{
if(freenectDevice_->isRunning())
{
freenectDevice_->getData(rgb, depth);
UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
depthConstant = 1.0f/freenectDevice_->getDepthFocal();
if(depth.empty())
{
UWARN("CameraFreenect: Data not ready! Try to reduce the image rate to avoid this warning...");
}
}
else
{
UERROR("CameraFreenect: Re-initialization needed!");
delete freenectDevice_;
freenectDevice_ = 0;
}
if(depth.empty() || rgb.empty())
{
rgb = cv::Mat();
depth = cv::Mat();
depthConstant = 0.0f;
}
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif
}
} // namespace rtabmap

76
corelib/src/CameraThread.cpp
View File

@@ -19,42 +19,70 @@
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/ParamEvent.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap
{
// ownership transferred
CameraThread::CameraThread(Camera * camera, bool autoRestart) :
CameraThread::CameraThread(Camera * camera) :
_camera(camera),
_autoRestart(autoRestart),
_cameraRGBD(0),
_seq(0)
{
UASSERT(_camera != 0);
}
// ownership transferred
CameraThread::CameraThread(CameraRGBD * camera) :
_camera(0),
_cameraRGBD(camera),
_seq(0)
{
UASSERT(_cameraRGBD != 0);
}
CameraThread::~CameraThread()
{
join(true);
delete _camera;
if(_camera)
{
delete _camera;
}
if(_cameraRGBD)
{
delete _cameraRGBD;
}
}
void CameraThread::setImageRate(float imageRate)
{
if(_camera)
{
_camera->setImageRate(imageRate);
}
if(_cameraRGBD)
{
_cameraRGBD->setImageRate(imageRate);
}
}
bool CameraThread::init()
{
if(!this->isRunning())
{
if(_camera)
_seq = 0;
if(_cameraRGBD)
{
_seq = 0;
return _camera->init();
return _cameraRGBD->init();
}
else
{
UERROR("Cannot initialize the camera because the camera object is null...");
return _camera->init();
}
}
else
@@ -67,29 +95,37 @@ bool CameraThread::init()
void CameraThread::mainLoop()
{
UTimer timer;
ULOGGER_DEBUG("Camera::process()");
UDEBUG("");
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat rgb, depth;
float depthConstant = 0.0f;
_camera->takeImage(rgb, depth, depthConstant);
if(_cameraRGBD)
{
_cameraRGBD->takeImage(rgb, depth, depthConstant);
}
else
{
rgb = _camera->takeImage();
}
if(!rgb.empty() && !this->isKilled())
{
this->post(new CameraEvent(rgb, depth, depthConstant, _camera->getLocalTransform(), ++_seq));
}
else if(!this->isKilled())
{
if(_autoRestart)
if(_cameraRGBD)
{
_camera->init();
this->post(new CameraEvent(rgb, depth, depthConstant, _cameraRGBD->getLocalTransform(), ++_seq));
}
else
{
ULOGGER_DEBUG("Camera::process() : no more images...");
this->kill();
this->post(new CameraEvent());
this->post(new CameraEvent(rgb, ++_seq));
}
}
else if(!this->isKilled())
{
UDEBUG("no more images...");
this->kill();
this->post(new CameraEvent());
}
}
} // namespace rtabmap

4
corelib/src/Features2d.cpp
View File

@@ -54,7 +54,7 @@ void filterKeypointsByDepth(
float depthConstant,
float maxDepth)
{
if(!depth.empty() && depthConstant > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == keypoints.size()))
if(!depth.empty() && depthConstant > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
{
std::vector<cv::KeyPoint> output(keypoints.size());
std::vector<int> indexes(keypoints.size(), 0);
@@ -71,7 +71,7 @@ void filterKeypointsByDepth(
output.resize(oi);
keypoints = output;
if(!descriptors.empty() && keypoints.size() != descriptors.rows)
if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
{
if(keypoints.size() == 0)
{