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

Refactored Camera classes and Preferences->Source menu

Browse Source
This commit is contained in:
matlabbe
2015-06-26 18:21:32 -04:00
parent 6f1df94b18
commit 6df403ed42
39 changed files with 3543 additions and 3388 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraThread.cpp
+31 -83
View File
@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraEvent.h"
#include <rtabmap/utilite/UTimer.h>
@@ -39,21 +38,12 @@ namespace rtabmap
// ownership transferred
CameraThread::CameraThread(Camera * camera) :
_camera(camera),
_cameraRGBD(0),
_seq(0)
_mirroring(false),
_colorOnly(false)
{
UASSERT(_camera != 0);
}
// ownership transferred
CameraThread::CameraThread(CameraRGBD * camera) :
_camera(0),
_cameraRGBD(camera),
_seq(0)
{
UASSERT(_cameraRGBD != 0);
}
CameraThread::~CameraThread()
{
join(true);
@@ -61,10 +51,6 @@ CameraThread::~CameraThread()
{
delete _camera;
}
if(_cameraRGBD)
{
delete _cameraRGBD;
}
}
void CameraThread::setImageRate(float imageRate)
@@ -73,86 +59,48 @@ void CameraThread::setImageRate(float imageRate)
{
_camera->setImageRate(imageRate);
}
if(_cameraRGBD)
{
_cameraRGBD->setImageRate(imageRate);
}
}
bool CameraThread::init()
{
if(!this->isRunning())
{
_seq = 0;
if(_cameraRGBD)
{
return _cameraRGBD->init();
}
else
{
return _camera->init();
}
// Added sleep time to ignore first frames (which are darker)
uSleep(1000);
}
else
{
UERROR("Cannot initialize the camera because it is already running...");
}
return false;
}
void CameraThread::mainLoop()
{
UTimer timer;
UDEBUG("");
cv::Mat rgb, depth;
float fx = 0.0f;
float fyOrBaseline = 0.0f;
float cx = 0.0f;
float cy = 0.0f;
double stamp = UTimer::now();
if(_cameraRGBD)
{
_cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy, stamp);
}
else
{
rgb = _camera->takeImage();
}
SensorData data = _camera->takeImage();
if(!rgb.empty())
if(!data.imageRaw().empty())
{
if(_cameraRGBD)
{
SensorData data;
if(dynamic_cast<CameraStereoFlyCapture2*>(_cameraRGBD) ||
dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) ||
dynamic_cast<CameraStereoImages*>(_cameraRGBD))
{
//stereo
data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
UASSERT(data.stereoCameraModel().isValid());
}
else
{
data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid());
}
this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
}
else
if(_colorOnly && !data.depthRaw().empty())
{
this->post(new CameraEvent(rgb, ++_seq, stamp));
data.setDepthOrRightRaw(cv::Mat());
}
if(_mirroring && data.cameraModels().size() == 1)
{
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
data.setImageRaw(tmpRgb);
if(data.cameraModels()[0].cx())
{
CameraModel tmpModel(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].localTransform());
data.setCameraModel(tmpModel);
}
if(!data.depthRaw().empty())
{
cv::Mat tmpDepth;
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
}
this->post(new CameraEvent(data, _camera->getSerial()));
}
else if(!this->isKilled())
{
if(_cameraRGBD)
{
UWARN("no more images...");
}
UWARN("no more images...");
this->kill();
this->post(new CameraEvent());
}