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

Added CameraRGBD with openni from OpenCV, removed feature2D stuff from Camera

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1358 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-13 03:47:39 +00:00
parent f423b1b743
commit 0bba363f5f
11 changed files with 345 additions and 309 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
corelib/include/rtabmap/core/Camera.h
View File

@@ -22,10 +22,7 @@
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/Image.h"
#include "rtabmap/core/Features2d.h"
#include <set>
#include <stack>
#include <list>
@@ -37,9 +34,6 @@ class UTimer;
namespace rtabmap
{
class KeypointDetector;
class KeypointDescriptor;
/**
* Class Camera
*
@@ -48,24 +42,20 @@ class RTABMAP_EXP Camera
{
public:
virtual ~Camera();
cv::Mat takeImage();
cv::Mat takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & keypoints);
cv::Mat takeImage(); // backward compatibility
void takeImage(cv::Mat & rgb);
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
virtual bool init() = 0;
//getters
void getImageSize(unsigned int & width, unsigned int & height);
float getImageRate() const {return _imageRate;}
bool isFeaturesExtracted() const {return _featuresExtracted;}
const Transform & getLocalTransform() const {return _localTransform;}
//setters
void setFeaturesExtracted(bool featuresExtracted,
KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef,
KeypointDescriptor::DescriptorType descriptor = KeypointDescriptor::kDescriptorUndef);
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setImageSize(unsigned int width, unsigned int height);
virtual void parseParameters(const ParametersMap & parameters);
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
protected:
/**
@@ -78,18 +68,15 @@ protected:
unsigned int imageHeight = 0,
unsigned int framesDropped = 0);
virtual cv::Mat captureImage() = 0;
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
private:
float _imageRate;
unsigned int _imageWidth;
unsigned int _imageHeight;
unsigned int _framesDropped;
Transform _localTransform;
UTimer * _frameRateTimer;
bool _featuresExtracted;
KeypointDetector * _keypointDetector;
KeypointDescriptor * _keypointDescriptor;
};
@@ -113,7 +100,7 @@ public:
std::string getPath() const {return _path;}
protected:
virtual cv::Mat captureImage();
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
private:
std::string _path;
@@ -156,7 +143,7 @@ public:
const std::string & getFilePath() const {return _filePath;}
protected:
virtual cv::Mat captureImage();
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
private:
// File type
@@ -170,4 +157,28 @@ private:
};
/////////////////////////
// CameraRGBD
/////////////////////////
class RTABMAP_EXP CameraRGBD :
public Camera
{
public:
CameraRGBD(float imageRate = 0,
bool asus = false);
virtual ~CameraRGBD();
virtual bool init();
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
private:
bool _asus;
cv::VideoCapture _capture;
};
} // namespace rtabmap

13
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -39,11 +39,8 @@ class Camera;
*/
class RTABMAP_EXP CameraThread :
public UThreadNode,
public UEventsHandler
public UEventsSender
{
public:
enum State {kStateCapturing, kStateChangingParameters};
public:
// ownership transferred
CameraThread(Camera * camera, bool autoRestart = false);
@@ -57,19 +54,11 @@ public:
void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
Camera * getCamera() {return _camera;}
protected:
virtual void handleEvent(UEvent* anEvent);
private:
virtual void mainLoop();
void process();
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
private:
Camera * _camera;
UMutex _stateMutex;
std::stack<State> _state;
std::stack<ParametersMap> _stateParam;
bool _autoRestart;
int _seq;
};

262
corelib/src/Camera.cpp
View File

@@ -19,7 +19,6 @@
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/Features2d.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
@@ -31,6 +30,8 @@
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
namespace rtabmap
{
@@ -42,40 +43,19 @@ Camera::Camera(float imageRate,
_imageWidth(imageWidth),
_imageHeight(imageHeight),
_framesDropped(framesDropped),
_frameRateTimer(new UTimer()),
_featuresExtracted(false),
_keypointDetector(0),
_keypointDescriptor(0)
_localTransform(Transform::getIdentity()),
_frameRateTimer(new UTimer())
{
}
Camera::~Camera()
{
if(_keypointDetector)
{
delete _keypointDetector;
}
if(_keypointDescriptor)
{
delete _keypointDescriptor;
}
if(_frameRateTimer)
{
delete _frameRateTimer;
}
}
void Camera::setFeaturesExtracted(bool featuresExtracted, KeypointDetector::DetectorType detector, KeypointDescriptor::DescriptorType descriptor)
{
_featuresExtracted = featuresExtracted;
if(detector != KeypointDetector::kDetectorUndef || descriptor != KeypointDescriptor::kDescriptorUndef)
{
ParametersMap pm;
pm.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str((int)detector)));
this->parseParameters(pm);
}
}
void Camera::setImageSize(unsigned int width, unsigned int height)
{
_imageWidth = width;
@@ -88,71 +68,23 @@ void Camera::getImageSize(unsigned int & width, unsigned int & height)
height = _imageHeight;
}
void Camera::parseParameters(const ParametersMap & parameters)
{
UDEBUG("");
ParametersMap::const_iterator iter;
//Keypoint detector
KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
{
detector = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
}
if(detector!=KeypointDetector::kDetectorUndef)
{
ULOGGER_DEBUG("new detector strategy %d", int(detector));
if(_keypointDetector)
{
delete _keypointDetector;
_keypointDetector = 0;
}
if(_keypointDescriptor)
{
delete _keypointDescriptor;
_keypointDescriptor = 0;
}
switch(detector)
{
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
_keypointDescriptor = new SURFDescriptor(parameters);
break;
}
}
else
{
if(_keypointDetector)
{
_keypointDetector->parseParameters(parameters);
}
if(_keypointDescriptor)
{
_keypointDescriptor->parseParameters(parameters);
}
}
}
cv::Mat Camera::takeImage()
{
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
bool tmp = _featuresExtracted;
_featuresExtracted = false; // no need to extract descriptors/keypoints in this function
cv::Mat img = takeImage(descriptors, keypoints);
_featuresExtracted = tmp;
return img;
cv::Mat rgb, depth;
float depthConstant = 0.0f;
takeImage(rgb, depth, depthConstant);
return rgb;
}
cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & keypoints)
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)
{
descriptors = cv::Mat();
keypoints.clear();
float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
if(imageRate>0)
{
@@ -173,35 +105,22 @@ cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & key
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
}
cv::Mat img;
UTimer timer;
img = this->captureImage();
this->captureImage(rgb, depth, depthConstant);
UDEBUG("Time capturing image = %fs", timer.ticks());
if(!img.empty())
if(!rgb.empty())
{
if(img.depth() != CV_8U)
{
UWARN("Images should have already 8U depth !?");
cv::Mat tmp = img;
img = cv::Mat();
tmp.convertTo(img, CV_8U);
UDEBUG("Time converting image to 8U = %fs", timer.ticks());
}
if(_featuresExtracted && _keypointDetector && _keypointDescriptor)
{
keypoints = _keypointDetector->generateKeypoints(img);
descriptors = _keypointDescriptor->generateDescriptors(img, keypoints);
UDEBUG("Post treatment time = %fs", timer.ticks());
}
UASSERT(rgb.depth() == CV_8U);
if(_framesDropped)
{
unsigned int count = 0;
while(count++ < _framesDropped)
{
cv::Mat tmp = this->captureImage();
cv::Mat tmp,tmp2;
float tmpf;
this->captureImage(tmp, tmp2, tmpf);
if(!tmp.empty())
{
UDEBUG("frame dropped (%d/%d)", (int)count, (int)_framesDropped);
@@ -214,7 +133,6 @@ cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & key
UDEBUG("Frames dropped time = %fs", timer.ticks());
}
}
return img;
}
/////////////////////////
@@ -272,10 +190,9 @@ bool CameraImages::init()
return _dir->isValid();
}
cv::Mat CameraImages::captureImage()
void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
UDEBUG("");
cv::Mat img;
if(_dir->isValid())
{
if(_refreshDir)
@@ -291,7 +208,7 @@ cv::Mat CameraImages::captureImage()
{
_lastFileName = *fileNames.rbegin();
std::string fullPath = _path + _lastFileName;
img = cv::imread(fullPath.c_str());
rgb = cv::imread(fullPath.c_str());
}
}
}
@@ -311,19 +228,19 @@ cv::Mat CameraImages::captureImage()
{
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
rgb = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
#else
img = cv::imread(fullPath.c_str(), -1);
rgb = cv::imread(fullPath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", img.cols, img.rows, img.channels(), img.elemSize(), img.total());
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", rgb.cols, rgb.rows, rgb.channels(), rgb.elemSize(), rgb.total());
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(img.depth() != CV_8U)
if(rgb.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());
img = cv::Mat(i, true);
rgb = cv::Mat(i, true);
cvReleaseImage(&i);
}
}
@@ -339,18 +256,16 @@ cv::Mat CameraImages::captureImage()
unsigned int h;
this->getImageSize(w, h);
if(!img.empty() &&
if(!rgb.empty() &&
w &&
h &&
w != (unsigned int)img.cols &&
h != (unsigned int)img.rows)
w != (unsigned int)rgb.cols &&
h != (unsigned int)rgb.rows)
{
cv::Mat resampled;
cv::resize(img, resampled, cv::Size(w, h));
img = resampled;
cv::resize(rgb, resampled, cv::Size(w, h));
rgb = resampled;
}
return img;
}
@@ -427,42 +342,115 @@ bool CameraVideo::init()
return true;
}
cv::Mat CameraVideo::captureImage()
void CameraVideo::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
{
cv::Mat img; // Null image
if(_capture.isOpened())
{
if(!_capture.read(img))
if(_capture.read(rgb))
{
if(_usbDevice)
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
if(!rgb.empty() &&
w &&
h &&
w != (unsigned int)rgb.cols &&
h != (unsigned int)rgb.rows)
{
UERROR("Camera has been disconnected!");
cv::Mat resampled;
cv::resize(rgb, resampled, cv::Size(w, h));
rgb = resampled;
}
else
{
// clone required
rgb = rgb.clone();
}
}
else if(_usbDevice)
{
UERROR("Camera has been disconnected!");
}
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
}
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
/////////////////////////
// CameraRGBD
/////////////////////////
CameraRGBD::CameraRGBD(float imageRate, bool asus) :
Camera(imageRate),
_asus(asus)
{
if(!img.empty() &&
w &&
h &&
w != (unsigned int)img.cols &&
h != (unsigned int)img.rows)
}
CameraRGBD::~CameraRGBD()
{
_capture.release();
}
bool CameraRGBD::init()
{
if(_capture.isOpened())
{
cv::Mat resampled;
cv::resize(img, resampled, cv::Size(w, h));
return resampled;
_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 );
// Print some avalible device settings.
UINFO("Depth generator output mode:");
UINFO("FRAME_WIDTH %d", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %d", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FRAME_MAX_DEPTH %d mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
UINFO("FPS %d", _capture.get( CV_CAP_PROP_FPS ));
UINFO("REGISTRATION %d", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
{
UINFO("Image generator output mode:");
UINFO("FRAME_WIDTH %d", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %d", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FPS %d", _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
{
// clone required
return img.clone();
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();
depthConstant = 0.001905f;
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
}

73
corelib/src/CameraThread.cpp
View File

@@ -39,7 +39,6 @@ CameraThread::CameraThread(Camera * camera, bool autoRestart) :
CameraThread::~CameraThread()
{
UEventsManager::removeHandler(this);
join(true);
delete _camera;
}
@@ -66,79 +65,17 @@ bool CameraThread::init()
}
void CameraThread::mainLoop()
{
State state = kStateCapturing;
ParametersMap parameters;
_stateMutex.lock();
{
if(!_state.empty() && !_stateParam.empty())
{
state = _state.top();
_state.pop();
parameters = _stateParam.top();
_stateParam.pop();
}
}
_stateMutex.unlock();
if(state == kStateCapturing)
{
process();
}
else if(state == kStateChangingParameters)
{
_camera->parseParameters(parameters);
}
}
void CameraThread::pushNewState(State newState, const ParametersMap & parameters)
{
ULOGGER_DEBUG("to %d", newState);
_stateMutex.lock();
{
_state.push(newState);
_stateParam.push(parameters);
}
_stateMutex.unlock();
}
void CameraThread::handleEvent(UEvent* anEvent)
{
if(anEvent->getClassName().compare("ParamEvent") == 0)
{
if(this->isIdle())
{
_stateMutex.lock();
_camera->parseParameters(((ParamEvent*)anEvent)->getParameters());
_stateMutex.unlock();
}
else
{
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)anEvent)->getParameters());
}
}
}
void CameraThread::process()
{
UTimer timer;
ULOGGER_DEBUG("Camera::process()");
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat img = _camera->takeImage(descriptors, keypoints);
if(!img.empty() && !this->isKilled())
cv::Mat rgb, depth;
float depthConstant = 0.0f;
_camera->takeImage(rgb, depth, depthConstant);
if(!rgb.empty() && !this->isKilled())
{
if(_camera->isFeaturesExtracted())
{
this->post(new CameraEvent(descriptors, keypoints, img, ++_seq));
}
else
{
this->post(new CameraEvent(img, ++_seq));
}
this->post(new CameraEvent(rgb, depth, depthConstant, _camera->getLocalTransform(), ++_seq));
}
else if(!this->isKilled())
{