i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-11 22:10:21 +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
corelib/include/rtabmap/core/Camera.h
+33 -22
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
corelib/include/rtabmap/core/CameraThread.h
+1 -12
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;
};
corelib/src/Camera.cpp
+125 -137
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.");
}
}
corelib/src/CameraThread.cpp
+5 -68
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())
{
guilib/include/rtabmap/gui/MainWindow.h
+3 -1
View File
@@ -115,6 +115,8 @@ private slots:
void selectDatabase();
void selectOpenni();
void selectFreenect();
void selectOpenniCv();
void selectOpenniCvAsus();
void dumpTheMemory();
void dumpThePrediction();
void downloadAllClouds();
@@ -177,7 +179,7 @@ private:
void setupMainLayout(bool vertical);
void updateSelectSourceImageMenu(int type);
void updateSelectSourceDatabase(bool used);
void updateSelectSourceOpenniMenu(bool used, bool openni);
void updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createAssembledCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
guilib/include/rtabmap/gui/PreferencesDialog.h
+9 -4
View File
@@ -52,6 +52,7 @@ namespace rtabmap {
class CameraOpenni;
class CameraFreenect;
class OdometryThread;
class CameraThread;
class Signature;
class LoopClosureViewer;
@@ -75,7 +76,11 @@ public:
kSrcUndef,
kSrcUsbDevice,
kSrcImages,
kSrcVideo
kSrcVideo,
kSrcOpenNI_PCL,
kSrcFreenect,
kSrcOpenNI_CV,
kSrcOpenNI_CV_ASUS
};
enum OdomType {
@@ -146,7 +151,6 @@ public:
bool isSourceOpenniUsed() const;
OdomType getOdometryType() const;
bool getGeneralAutoRestart() const;
bool getGeneralCameraKeypoints() const;
int getSourceImageType() const;
QString getSourceImageTypeStr() const;
int getSourceWidth() const;
@@ -162,7 +166,7 @@ public:
QString getSourceDatabasePath() const; //Database group
bool getSourceDatabaseOdometryIgnored() const; //Database group
int getSourceDatabaseStartPos() const; //Database group
bool getSourceFreenect() const; // Openni group
Src getSourceRGBD() const; // Openni group
QString getSourceOpenniDevice() const; //Openni group
Transform getSourceOpenniLocalTransform() const; //Openni group
@@ -196,7 +200,7 @@ public slots:
void setSLAMMode(bool enabled);
void selectSourceImage(Src src = kSrcUndef);
void selectSourceDatabase(bool user = false);
void selectSourceOpenni(bool openni);
void selectSourceRGBD(Src src = kSrcUndef);
private slots:
void closeDialog ( QAbstractButton * button );
@@ -276,6 +280,7 @@ private:
//Odometry test
CameraOpenni * _odomCameraOpenNI;
CameraFreenect * _odomCameraFreenect;
CameraThread * _odomCameraOpenNICv;
OdometryThread * _odomThread;
QVector<QCheckBox*> _3dRenderingShowClouds;
guilib/src/MainWindow.cpp
+54 -25
View File
@@ -290,9 +290,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
this->updateSelectSourceDatabase(_preferencesDialog->isSourceDatabaseUsed());
connect(_ui->actionDatabase, SIGNAL(triggered()), this, SLOT(selectDatabase()));
this->updateSelectSourceOpenniMenu(_preferencesDialog->isSourceOpenniUsed(), !_preferencesDialog->getSourceFreenect());
this->updateSelectSourceRGBDMenu(_preferencesDialog->isSourceOpenniUsed(), _preferencesDialog->getSourceRGBD());
connect(_ui->actionOpenNI, SIGNAL(triggered()), this, SLOT(selectOpenni()));
connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures()));
@@ -1514,7 +1516,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
_ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
this->updateSelectSourceImageMenu(_preferencesDialog->getSourceImageType());
this->updateSelectSourceDatabase(_preferencesDialog->isSourceDatabaseUsed());
this->updateSelectSourceOpenniMenu(_preferencesDialog->isSourceOpenniUsed(), !_preferencesDialog->getSourceFreenect());
this->updateSelectSourceRGBDMenu(_preferencesDialog->isSourceOpenniUsed(), _preferencesDialog->getSourceRGBD());
QString src;
if(_preferencesDialog->isSourceImageUsed())
{
@@ -1830,10 +1832,12 @@ void MainWindow::updateSelectSourceDatabase(bool used)
_ui->actionDatabase->setChecked(used);
}
void MainWindow::updateSelectSourceOpenniMenu(bool used, bool openni)
void MainWindow::updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src)
{
_ui->actionOpenNI->setChecked(used && openni);
_ui->actionFreenect->setChecked(used && !openni);
_ui->actionOpenNI->setChecked(used && src == PreferencesDialog::kSrcOpenNI_PCL);
_ui->actionFreenect->setChecked(used && src == PreferencesDialog::kSrcFreenect);
_ui->actionOpenNI_CV->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV);
_ui->actionOpenNI_CV_ASUS->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV_ASUS);
}
void MainWindow::changeImgRateSetting()
@@ -2023,7 +2027,7 @@ void MainWindow::startDetection()
_odomThread->start();
}
if(_preferencesDialog->getSourceFreenect())
if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
{
_cameraOpenKinect = new CameraFreenect(
_preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()),
@@ -2060,6 +2064,37 @@ void MainWindow::startDetection()
UEventsManager::createPipe(_cameraOpenKinect, _odomThread, "CameraEvent");
}
}
else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
{
Camera * camera = new CameraRGBD(
_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
camera->setLocalTransform(_preferencesDialog->getSourceOpenniLocalTransform());
if(!camera->init())
{
ULOGGER_WARN("init camera failed... ");
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Camera initialization failed..."));
emit stateChanged(kIdle);
delete camera;
camera = 0;
if(_odomThread)
{
delete _odomThread;
_odomThread = 0;
}
return;
}
_camera = new CameraThread(camera);
if(_odomThread)
{
UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
}
}
else
{
_cameraOpenni = new CameraOpenni(
@@ -2176,7 +2211,7 @@ void MainWindow::startDetection()
_preferencesDialog->getSourceHeight(),
_preferencesDialog->getFramesDropped());
}
else if(sourceType == 0)
else //if(sourceType == 0)
{
camera = new CameraVideo(
_preferencesDialog->getSourceUsbDeviceId(),
@@ -2185,21 +2220,6 @@ void MainWindow::startDetection()
_preferencesDialog->getSourceHeight(),
_preferencesDialog->getFramesDropped());
}
else
{
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Source type is not supported..."));
ULOGGER_WARN("iSource type not supported...");
emit stateChanged(kIdle);
return;
}
if(_preferencesDialog->getGeneralCameraKeypoints())
{
camera->setFeaturesExtracted(true);
camera->parseParameters(_preferencesDialog->getAllParameters());
}
if(!camera->init())
{
@@ -2214,7 +2234,6 @@ void MainWindow::startDetection()
}
_camera = new CameraThread(camera);
UEventsManager::addHandler(_camera); //thread
}
}
@@ -2580,12 +2599,22 @@ void MainWindow::selectDatabase()
void MainWindow::selectOpenni()
{
_preferencesDialog->selectSourceOpenni(true);
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL);
}
void MainWindow::selectFreenect()
{
_preferencesDialog->selectSourceOpenni(false);
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect);
}
void MainWindow::selectOpenniCv()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV);
}
void MainWindow::selectOpenniCvAsus()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS);
}
guilib/src/PreferencesDialog.cpp
+71 -22
View File
@@ -40,6 +40,8 @@
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/CameraOpenni.h"
#include "rtabmap/core/CameraFreenect.h"
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/gui/LoopClosureViewer.h"
@@ -64,6 +66,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_initialized(false),
_odomCameraOpenNI(0),
_odomCameraFreenect(0),
_odomCameraOpenNICv(0),
_odomThread(0)
{
ULOGGER_DEBUG("");
@@ -208,7 +211,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_image, SLOT(setCurrentIndex(int)));
connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->general_checkBox_autoRestart, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->general_checkBox_cameraKeypoints, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//usbDevice group
connect(_ui->source_usbDevice_spinBox_id, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_imgWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -231,8 +233,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//openni group
connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->radioButton_openni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->radioButton_opennipcl, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->radioButton_freenect, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->radioButton_opennicv, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->radioButton_opennicvasus, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -758,7 +762,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->source_spinBox_imgheight->setValue(0);
_ui->source_spinBox_framesDropped->setValue(0);
_ui->general_checkBox_autoRestart->setChecked(false);
_ui->general_checkBox_cameraKeypoints->setChecked(false);
_ui->source_images_spinBox_startPos->setValue(1);
_ui->source_images_refreshDir->setChecked(false);
@@ -767,8 +770,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->source_spinBox_databaseStartPos->setValue(0);
_ui->groupBox_sourceOpenni->setChecked(true);
_ui->radioButton_openni->setChecked(true);
_ui->radioButton_opennipcl->setChecked(true);
_ui->radioButton_freenect->setChecked(false);
_ui->radioButton_opennicv->setChecked(false);
_ui->radioButton_opennicvasus->setChecked(false);
_ui->lineEdit_openniDevice->setText("");
_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
}
@@ -978,7 +983,6 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->groupBox_sourceImage->setChecked(settings.value("imageUsed", _ui->groupBox_sourceImage->isChecked()).toBool());
_ui->general_doubleSpinBox_imgRate->setValue(settings.value("imgRate", _ui->general_doubleSpinBox_imgRate->value()).toDouble());
_ui->general_checkBox_autoRestart->setChecked(settings.value("autoRestart", _ui->general_checkBox_autoRestart->isChecked()).toBool());
_ui->general_checkBox_cameraKeypoints->setChecked(settings.value("cameraKeypoints", _ui->general_checkBox_cameraKeypoints->isChecked()).toBool());
_ui->source_comboBox_image_type->setCurrentIndex(settings.value("type", _ui->source_comboBox_image_type->currentIndex()).toInt());
_ui->source_spinBox_imgWidth->setValue(settings.value("imgWidth",_ui->source_spinBox_imgWidth->value()).toInt());
_ui->source_spinBox_imgheight->setValue(settings.value("imgHeight",_ui->source_spinBox_imgheight->value()).toInt());
@@ -1008,8 +1012,10 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("Openni");
_ui->groupBox_sourceOpenni->setChecked(settings.value("openniUsed", _ui->groupBox_sourceOpenni->isChecked()).toBool());
_ui->radioButton_openni->setChecked(settings.value("openniType", _ui->radioButton_openni->isChecked()).toBool());
_ui->radioButton_opennipcl->setChecked(settings.value("openniType", _ui->radioButton_opennipcl->isChecked()).toBool());
_ui->radioButton_freenect->setChecked(settings.value("freenectType", _ui->radioButton_freenect->isChecked()).toBool());
_ui->radioButton_opennicv->setChecked(settings.value("openniCvType", _ui->radioButton_opennicv->isChecked()).toBool());
_ui->radioButton_opennicvasus->setChecked(settings.value("openniCvAsusType", _ui->radioButton_opennicvasus->isChecked()).toBool());
_ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString());
_ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString());
settings.endGroup(); // Openni
@@ -1186,7 +1192,6 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
settings.setValue("imageUsed", _ui->groupBox_sourceImage->isChecked());
settings.setValue("imgRate", _ui->general_doubleSpinBox_imgRate->value());
settings.setValue("autoRestart", _ui->general_checkBox_autoRestart->isChecked());
settings.setValue("cameraKeypoints", _ui->general_checkBox_cameraKeypoints->isChecked());
settings.setValue("type", _ui->source_comboBox_image_type->currentIndex());
settings.setValue("imgWidth", _ui->source_spinBox_imgWidth->value());
settings.setValue("imgHeight", _ui->source_spinBox_imgheight->value());
@@ -1217,8 +1222,10 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
settings.beginGroup("Openni");
settings.setValue("openniUsed", _ui->groupBox_sourceOpenni->isChecked());
settings.setValue("openniType", _ui->radioButton_openni->isChecked());
settings.setValue("openniType", _ui->radioButton_opennipcl->isChecked());
settings.setValue("freenectType", _ui->radioButton_freenect->isChecked());
settings.setValue("openniCvType", _ui->radioButton_opennicv->isChecked());
settings.setValue("openniCvAsusType", _ui->radioButton_opennicvasus->isChecked());
settings.setValue("device", _ui->lineEdit_openniDevice->text());
settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
settings.endGroup();
@@ -1581,7 +1588,7 @@ void PreferencesDialog::selectSourceDatabase(bool user)
}
}
void PreferencesDialog::selectSourceOpenni(bool openni)
void PreferencesDialog::selectSourceRGBD(Src src)
{
ULOGGER_DEBUG("");
@@ -1589,8 +1596,8 @@ void PreferencesDialog::selectSourceOpenni(bool openni)
{
int button = QMessageBox::information(this,
tr("Activate RGB-D SLAM?"),
tr("You've selected %1 camera as source input, "
"would you want to activate RGB-D SLAM mode?").arg(openni?"OpenNI":"Freenect"),
tr("You've selected RGB-D camera as source input, "
"would you want to activate RGB-D SLAM mode?"),
QMessageBox::Yes | QMessageBox::No);
if(button & QMessageBox::Yes)
{
@@ -1599,8 +1606,10 @@ void PreferencesDialog::selectSourceOpenni(bool openni)
}
_ui->groupBox_sourceOpenni->setChecked(true);
_ui->radioButton_openni->setChecked(openni);
_ui->radioButton_freenect->setChecked(!openni);
_ui->radioButton_opennipcl->setChecked(src == kSrcOpenNI_PCL);
_ui->radioButton_freenect->setChecked(src == kSrcFreenect);
_ui->radioButton_opennicv->setChecked(src == kSrcOpenNI_CV);
_ui->radioButton_opennicvasus->setChecked(src == kSrcOpenNI_CV_ASUS);
if(_obsoletePanels)
{
@@ -2456,10 +2465,6 @@ bool PreferencesDialog::getGeneralAutoRestart() const
{
return _ui->general_checkBox_autoRestart->isChecked();
}
bool PreferencesDialog::getGeneralCameraKeypoints() const
{
return _ui->general_checkBox_cameraKeypoints->isChecked();
}
int PreferencesDialog::getSourceImageType() const
{
return _ui->source_comboBox_image_type->currentIndex();
@@ -2512,9 +2517,25 @@ int PreferencesDialog::getSourceDatabaseStartPos() const
{
return _ui->source_spinBox_databaseStartPos->value();
}
bool PreferencesDialog::getSourceFreenect() const
PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const
{
return _ui->radioButton_freenect->isChecked();
if(_ui->radioButton_freenect->isChecked())
{
return kSrcFreenect;
}
else if(_ui->radioButton_opennicv->isChecked())
{
return kSrcOpenNI_CV;
}
else if (_ui->radioButton_opennicvasus->isChecked())
{
return kSrcOpenNI_CV_ASUS;
}
else
{
return kSrcOpenNI_PCL;
}
}
QString PreferencesDialog::getSourceOpenniDevice() const
{
@@ -2704,9 +2725,9 @@ void PreferencesDialog::testOdometry()
void PreferencesDialog::testOdometry(OdomType type)
{
UASSERT(_odomCameraOpenNI == 0 && _odomCameraFreenect == 0 && _odomThread == 0);
UASSERT(_odomCameraOpenNI == 0 && _odomCameraFreenect == 0 && _odomThread == 0 && _odomCameraOpenNICv == 0);
if(this->getSourceFreenect())
if(this->getSourceRGBD() == kSrcFreenect)
{
_odomCameraFreenect = new CameraFreenect(
this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
@@ -2730,6 +2751,20 @@ void PreferencesDialog::testOdometry(OdomType type)
_odomCameraFreenect = 0;
}
}
else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
{
_odomCameraOpenNICv = new CameraThread(new CameraRGBD(
this->getGeneralInputRate(),
this->getSourceRGBD() == kSrcOpenNI_CV_ASUS));
if(!_odomCameraOpenNICv->init())
{
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("OpenNI-CV camera initialization failed!"));
delete _odomCameraOpenNICv;
_odomCameraOpenNICv = 0;
}
}
else
{
_odomCameraOpenNI = new CameraOpenni(
@@ -2746,7 +2781,7 @@ void PreferencesDialog::testOdometry(OdomType type)
}
}
if(_odomCameraOpenNI || _odomCameraFreenect)
if(_odomCameraOpenNI || _odomCameraFreenect || _odomCameraOpenNICv)
{
Odometry * odometry;
ParametersMap parameters = this->getAllParameters();
@@ -2786,6 +2821,10 @@ void PreferencesDialog::testOdometry(OdomType type)
{
UEventsManager::createPipe(_odomCameraFreenect, _odomThread, "CameraEvent");
}
else if(_odomCameraOpenNICv)
{
UEventsManager::createPipe(_odomCameraOpenNICv, _odomThread, "CameraEvent");
}
else
{
UEventsManager::createPipe(_odomCameraOpenNI, _odomThread, "CameraEvent");
@@ -2803,6 +2842,10 @@ void PreferencesDialog::testOdometry(OdomType type)
{
_odomCameraFreenect->start();
}
else if(_odomCameraOpenNICv)
{
_odomCameraOpenNICv->start();
}
else
{
_odomCameraOpenNI->start();
@@ -2825,6 +2868,12 @@ void PreferencesDialog::cleanOdometryTest()
delete _odomCameraFreenect;
_odomCameraFreenect = 0;
}
if(_odomCameraOpenNICv)
{
_odomCameraOpenNICv->join(true);
delete _odomCameraOpenNICv;
_odomCameraOpenNICv = 0;
}
if(_odomThread)
{
_odomThread->join(true);
guilib/src/ui/mainWindow.ui
+20 -2
View File
@@ -27,7 +27,7 @@
<x>0</x>
<y>0</y>
<width>947</width>
<height>25</height>
<height>21</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -92,6 +92,8 @@
</property>
<addaction name="actionOpenNI"/>
<addaction name="actionFreenect"/>
<addaction name="actionOpenNI_CV"/>
<addaction name="actionOpenNI_CV_ASUS"/>
</widget>
<addaction name="menuImage"/>
<addaction name="actionDatabase"/>
@@ -983,7 +985,7 @@
<bool>true</bool>
</property>
<property name="text">
<string>OpenNI</string>
<string>OpenNI-PCL</string>
</property>
</action>
<action name="actionFreenect">
@@ -994,6 +996,22 @@
<string>Freenect</string>
</property>
</action>
<action name="actionOpenNI_CV">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>OpenNI-CV</string>
</property>
</action>
<action name="actionOpenNI_CV_ASUS">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>OpenNI-CV-ASUS</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/preferencesDialog.ui
+20 -6
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>729</width>
<height>836</height>
<y>-142</y>
<width>728</width>
<height>721</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>1</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1736,9 +1736,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<layout class="QHBoxLayout" name="horizontalLayout_7">
<item>
<widget class="QRadioButton" name="radioButton_openni">
<widget class="QRadioButton" name="radioButton_opennipcl">
<property name="text">
<string>OpenNI</string>
<string>OpenNI-PCL</string>
</property>
</widget>
</item>
@@ -1749,6 +1749,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="radioButton_opennicv">
<property name="text">
<string>OpenNI-CV</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="radioButton_opennicvasus">
<property name="text">
<string>OpenNI-CV-ASUS</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
tools/ConsoleApp/main.cpp
+4 -10
View File
@@ -422,20 +422,14 @@ int main(int argc, char * argv[])
}
printf("\nProcessing images...\n");
//setup camera
camera->setFeaturesExtracted(true);
camera->parseParameters(pm);
UTimer iterationTimer;
UTimer rtabmapTimer;
int imagesProcessed = 0;
std::list<std::vector<float> > teleopActions;
while(loopDataset <= repeat && g_forever)
{
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat cvImg = camera->takeImage(descriptors, keypoints);
Image img(cvImg, 0, descriptors, keypoints);
cv::Mat cvImg = camera->takeImage();
Image img(cvImg, 0);
int i=0;
double maxIterationTime = 0.0;
int maxIterationTimeId = 0;
@@ -451,8 +445,8 @@ int main(int argc, char * argv[])
{
++countLoopDetected;
}
cvImg = camera->takeImage(descriptors, keypoints);
img = Image(cvImg, 0, descriptors, keypoints);
cvImg = camera->takeImage();
img = Image(cvImg, 0);
if(++count % 100 == 0)
{
printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",