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

DBReader now inherits from Camera (so that CameraThread's post processing stuff can be used with a database stream)

Browse Source
This commit is contained in:
matlabbe
2016-06-21 11:22:24 -04:00
parent cdddb1209e
commit 0ec39e3c77
20 changed files with 435 additions and 642 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/jni/CameraTango.cpp
+1 -1
View File
@@ -464,7 +464,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp, bool inOpen
return pose;
}
SensorData CameraTango::captureImage()
SensorData CameraTango::captureImage(CameraInfo * info)
{
LOGI("Capturing image...");
app/android/jni/CameraTango.h
+1 -1
View File
@@ -86,7 +86,7 @@ public:
void tangoEventReceived(int type, const char * key, const char * value);
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
rtabmap::Transform getPoseAtTimestamp(double timestamp, bool inOpenGLFrame);
corelib/include/rtabmap/core/Camera.h
+1 -1
View File
@@ -76,7 +76,7 @@ protected:
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureImage() = 0;
virtual SensorData captureImage(CameraInfo * info = 0) = 0;
int getNextSeqID() {return ++_seq;}
corelib/include/rtabmap/core/CameraInfo.h
+4 -1
View File
@@ -43,7 +43,8 @@ public:
timeDisparity(0.0f),
timeMirroring(0.0f),
timeImageDecimation(0.0f),
timeScanFromDepth(0.0f)
timeScanFromDepth(0.0f),
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
virtual ~CameraInfo() {}
@@ -55,6 +56,8 @@ public:
float timeMirroring;
float timeImageDecimation;
float timeScanFromDepth;
Transform odomPose;
cv::Mat odomCovariance;
};
} // namespace rtabmap
corelib/include/rtabmap/core/CameraRGB.h
+2 -2
View File
@@ -119,7 +119,7 @@ public:
}
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
std::string _path;
@@ -194,7 +194,7 @@ public:
const std::string & getFilePath() const {return _filePath;}
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
// File type
corelib/include/rtabmap/core/CameraRGBD.h
+6 -6
View File
@@ -97,7 +97,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
pcl::Grabber* interface_;
@@ -131,7 +131,7 @@ public:
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
bool _asus;
@@ -168,7 +168,7 @@ public:
void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
openni::Device * _device;
@@ -204,7 +204,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
int deviceId_;
@@ -249,7 +249,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
int deviceId_;
@@ -291,7 +291,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
CameraImages cameraDepth_;
corelib/include/rtabmap/core/CameraStereo.h
+5 -5
View File
@@ -70,7 +70,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
DC1394Device *device_;
@@ -95,7 +95,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
FlyCapture2::Camera * camera_;
@@ -134,7 +134,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
sl::zed::Camera * zed_;
@@ -177,7 +177,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
CameraImages * camera2_;
@@ -213,7 +213,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
cv::VideoCapture capture_;
corelib/include/rtabmap/core/DBReader.h
+20 -12
View File
@@ -30,11 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/utilite/UThreadNode.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UEventsSender.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/Camera.h>
#include <opencv2/core/core.hpp>
@@ -45,36 +43,45 @@ namespace rtabmap {
class DBDriver;
class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender {
class RTABMAP_EXP DBReader : public Camera {
public:
DBReader(const std::string & databasePath,
float frameRate = 0.0f,
float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
bool odometryIgnored = false,
bool ignoreGoalDelay = false,
bool goalsIgnored = false,
int startIndex = 0,
int cameraIndex = -1);
DBReader(const std::list<std::string> & databasePaths,
float frameRate = 0.0f,
float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
bool odometryIgnored = false,
bool ignoreGoalDelay = false,
bool goalsIgnored = false,
int startIndex = 0,
int cameraIndex = -1);
virtual ~DBReader();
bool init(int startIndex=0);
void setFrameRate(float frameRate);
OdometryEvent getNextData();
virtual bool init(
const std::string & calibrationFolder = ".",
const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
bool isOdometryIgnored() const {return _odometryIgnored;}
protected:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual SensorData captureImage(CameraInfo * info = 0);
private:
SensorData getNextData(CameraInfo * info = 0);
private:
std::list<std::string> _paths;
float _frameRate; // -1 = use Database stamps, 0 = inf
bool _odometryIgnored;
bool _ignoreGoalDelay;
bool _goalsIgnored;
int _startIndex;
int _cameraIndex;
DBDriver * _dbDriver;
@@ -83,6 +90,7 @@ private:
std::set<int>::iterator _currentId;
double _previousStamp;
int _previousMapID;
bool _calibrated;
};
} /* namespace rtabmap */
corelib/src/Camera.cpp
+1 -1
View File
@@ -88,7 +88,7 @@ SensorData Camera::takeImage(CameraInfo * info)
}
UTimer timer;
SensorData data = this->captureImage();
SensorData data = this->captureImage(info);
double captureTime = timer.ticks();
if(warnFrameRateTooHigh)
{
corelib/src/CameraRGB.cpp
+2 -2
View File
@@ -438,7 +438,7 @@ std::vector<std::string> CameraImages::filenames() const
return std::vector<std::string>();
}
SensorData CameraImages::captureImage()
SensorData CameraImages::captureImage(CameraInfo * info)
{
if(syncImageRateWithStamps_ && _captureDelay>0.0)
{
@@ -801,7 +801,7 @@ std::string CameraVideo::getSerial() const
return _guid;
}
SensorData CameraVideo::captureImage()
SensorData CameraVideo::captureImage(CameraInfo * info)
{
cv::Mat img;
if(_capture.isOpened())
corelib/src/CameraRGBD.cpp
+7 -7
View File
@@ -203,7 +203,7 @@ std::string CameraOpenni::getSerial() const
return "";
}
SensorData CameraOpenni::captureImage()
SensorData CameraOpenni::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef HAVE_OPENNI
@@ -317,7 +317,7 @@ bool CameraOpenNICV::isCalibrated() const
return true;
}
SensorData CameraOpenNICV::captureImage()
SensorData CameraOpenNICV::captureImage(CameraInfo * info)
{
SensorData data;
if(_capture.isOpened())
@@ -671,7 +671,7 @@ std::string CameraOpenNI2::getSerial() const
return "";
}
SensorData CameraOpenNI2::captureImage()
SensorData CameraOpenNI2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_OPENNI2
@@ -1033,7 +1033,7 @@ std::string CameraFreenect::getSerial() const
return "";
}
SensorData CameraFreenect::captureImage()
SensorData CameraFreenect::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_FREENECT
@@ -1307,7 +1307,7 @@ std::string CameraFreenect2::getSerial() const
return "";
}
SensorData CameraFreenect2::captureImage()
SensorData CameraFreenect2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_FREENECT2
@@ -1720,12 +1720,12 @@ std::string CameraRGBDImages::getSerial() const
return this->cameraModel().name();
}
SensorData CameraRGBDImages::captureImage()
SensorData CameraRGBDImages::captureImage(CameraInfo * info)
{
SensorData data;
SensorData rgb, depth;
rgb = CameraImages::captureImage();
rgb = CameraImages::captureImage(info);
if(!rgb.imageRaw().empty())
{
depth = cameraDepth_.takeImage();
corelib/src/CameraStereo.cpp
+8 -8
View File
@@ -415,7 +415,7 @@ std::string CameraStereoDC1394::getSerial() const
return "";
}
SensorData CameraStereoDC1394::captureImage()
SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_DC1394
@@ -610,7 +610,7 @@ struct ImageContainer
} ;
#endif
SensorData CameraStereoFlyCapture2::captureImage()
SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_FLYCAPTURE2
@@ -887,7 +887,7 @@ std::string CameraStereoZed::getSerial() const
return "";
}
SensorData CameraStereoZed::captureImage()
SensorData CameraStereoZed::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_ZED
@@ -1072,21 +1072,21 @@ std::string CameraStereoImages::getSerial() const
return stereoModel_.name();
}
SensorData CameraStereoImages::captureImage()
SensorData CameraStereoImages::captureImage(CameraInfo * info)
{
SensorData data;
SensorData left, right;
left = CameraImages::captureImage();
left = CameraImages::captureImage(info);
if(!left.imageRaw().empty())
{
if(camera2_)
{
right = camera2_->takeImage();
right = camera2_->takeImage(info);
}
else
{
right = this->takeImage();
right = this->takeImage(info);
}
if(!right.imageRaw().empty())
@@ -1235,7 +1235,7 @@ std::string CameraStereoVideo::getSerial() const
return cameraName_;
}
SensorData CameraStereoVideo::captureImage()
SensorData CameraStereoVideo::captureImage(CameraInfo * info)
{
SensorData data;
corelib/src/DBReader.cpp
+168 -169
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UEventsManager.h>
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/RtabmapEvent.h"
@@ -47,17 +48,20 @@ DBReader::DBReader(const std::string & databasePath,
bool odometryIgnored,
bool ignoreGoalDelay,
bool goalsIgnored,
int startIndex,
int cameraIndex) :
Camera(frameRate),
_paths(uSplit(databasePath, ';')),
_frameRate(frameRate),
_odometryIgnored(odometryIgnored),
_ignoreGoalDelay(ignoreGoalDelay),
_goalsIgnored(goalsIgnored),
_startIndex(startIndex),
_cameraIndex(cameraIndex),
_dbDriver(0),
_currentId(_ids.end()),
_previousStamp(0),
_previousMapID(0)
_previousMapID(0),
_calibrated(false)
{
}
@@ -66,17 +70,20 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
bool odometryIgnored,
bool ignoreGoalDelay,
bool goalsIgnored,
int startIndex,
int cameraIndex) :
Camera(frameRate),
_paths(databasePaths),
_frameRate(frameRate),
_odometryIgnored(odometryIgnored),
_ignoreGoalDelay(ignoreGoalDelay),
_goalsIgnored(goalsIgnored),
_startIndex(startIndex),
_cameraIndex(cameraIndex),
_dbDriver(0),
_currentId(_ids.end()),
_previousStamp(0),
_previousMapID(0)
_previousMapID(0),
_calibrated(false)
{
}
@@ -89,7 +96,9 @@ DBReader::~DBReader()
}
}
bool DBReader::init(int startIndex)
bool DBReader::init(
const std::string & calibrationFolder,
const std::string & cameraName)
{
if(_dbDriver)
{
@@ -101,6 +110,7 @@ bool DBReader::init(int startIndex)
_currentId=_ids.end();
_previousStamp = 0;
_previousMapID = 0;
_calibrated = false;
if(_paths.size() == 0)
{
@@ -133,12 +143,12 @@ bool DBReader::init(int startIndex)
_dbDriver->getAllNodeIds(_ids);
_currentId = _ids.begin();
if(startIndex>0 && _ids.size())
if(_startIndex>0 && _ids.size())
{
std::set<int>::iterator iter = uIteratorAt(_ids, startIndex);
std::set<int>::iterator iter = uIteratorAt(_ids, _startIndex);
if(iter == _ids.end())
{
UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, _ids.size()-1);
UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", _startIndex, _ids.size()-1);
}
else
{
@@ -146,142 +156,139 @@ bool DBReader::init(int startIndex)
}
}
if(_ids.size())
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
{
if(models.size() && models.at(0).isValidForProjection())
{
_calibrated = true;
}
else if(stereoModel.isValidForProjection())
{
_calibrated = true;
}
}
}
_timer.start();
return true;
}
void DBReader::setFrameRate(float frameRate)
bool DBReader::isCalibrated() const
{
_frameRate = frameRate;
return _calibrated;
}
void DBReader::mainLoopBegin()
std::string DBReader::getSerial() const
{
_timer.start();
return "DBReader";
}
void DBReader::mainLoop()
SensorData DBReader::captureImage(CameraInfo * info)
{
OdometryEvent odom = this->getNextData();
if(odom.data().id())
{
std::string goalId;
double previousStamp = odom.data().stamp();
if(previousStamp == 0)
{
odom.data().setStamp(UTimer::now());
}
if(!_goalsIgnored &&
odom.data().userDataRaw().type() == CV_8SC1 &&
odom.data().userDataRaw().cols >= 7 && // including null str ending
odom.data().userDataRaw().rows == 1 &&
memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = (const char *)odom.data().userDataRaw().data;
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
goalId = *strs.rbegin();
odom.data().setUserData(cv::Mat());
}
}
}
if(!_odometryIgnored)
{
if(odom.pose().isNull())
{
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
this->post(new OdometryEvent(odom));
}
else
{
this->post(new CameraEvent(odom.data()));
}
if(!goalId.empty())
{
double delay = 0.0;
if(!_ignoreGoalDelay && _currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
// that was used originally for planning
int weight;
std::string label;
double stamp;
int mapId;
Transform localTransform, pose, groundTruth;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
if(previousStamp && stamp && stamp > previousStamp)
{
delay = stamp - previousStamp;
}
}
if(delay > 0.0)
{
UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
goalId.c_str(), delay);
}
else
{
UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
}
if(uIsInteger(goalId))
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
}
if(delay > 0.0)
{
uSleep(delay*1000);
}
}
}
else if(!this->isKilled())
SensorData data = this->getNextData(info);
if(data.id() == 0)
{
UINFO("no more images...");
if(_paths.size() > 1)
while(_paths.size() > 1 && data.id() == 0)
{
_paths.pop_front();
UWARN("Loading next database \"%s\"...", _paths.front().c_str());
if(!this->init())
{
UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
this->kill();
this->post(new CameraEvent());
return data;
}
else
{
data = this->getNextData(info);
}
}
else
}
if(data.id())
{
std::string goalId;
double previousStamp = data.stamp();
if(previousStamp == 0)
{
this->kill();
this->post(new CameraEvent());
data.setStamp(UTimer::now());
}
}
if(!_goalsIgnored &&
data.userDataRaw().type() == CV_8SC1 &&
data.userDataRaw().cols >= 7 && // including null str ending
data.userDataRaw().rows == 1 &&
memcmp(data.userDataRaw().data, "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = (const char *)data.userDataRaw().data;
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
goalId = *strs.rbegin();
data.setUserData(cv::Mat());
double delay = 0.0;
if(!_ignoreGoalDelay && _currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
// that was used originally for planning
int weight;
std::string label;
double stamp;
int mapId;
Transform localTransform, pose, groundTruth;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
if(previousStamp && stamp && stamp > previousStamp)
{
delay = stamp - previousStamp;
}
}
if(delay > 0.0)
{
UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
goalId.c_str(), delay);
}
else
{
UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
}
if(uIsInteger(goalId))
{
UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
}
else
{
UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
}
if(delay > 0.0)
{
uSleep(delay*1000);
}
}
}
}
}
return data;
}
OdometryEvent DBReader::getNextData()
SensorData DBReader::getNextData(CameraInfo * info)
{
OdometryEvent odom;
SensorData data;
if(_dbDriver)
{
if(!this->isKilled() && _currentId != _ids.end())
if(_currentId != _ids.end())
{
int mapId;
SensorData data;
_dbDriver->getNodeData(*_currentId, data);
// info
@@ -316,12 +323,12 @@ OdometryEvent DBReader::getNextData()
}
// Frame rate
if(_frameRate < 0.0f)
if(this->getImageRate() < 0.0f)
{
if(stamp == 0)
{
UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
this->kill();
return data;
}
else if(_previousMapID == mapId && _previousStamp > 0)
{
@@ -350,69 +357,61 @@ OdometryEvent DBReader::getNextData()
_previousStamp = stamp;
_previousMapID = mapId;
}
else if(_frameRate>0.0f)
data.uncompressData();
if(data.cameraModels().size() > 1 &&
_cameraIndex >= 0)
{
int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
if(_cameraIndex < (int)data.cameraModels().size())
{
uSleep(sleepTime-2);
}
// select one camera
int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
UASSERT(!data.imageRaw().empty() &&
data.imageRaw().cols % data.cameraModels().size() == 0 &&
_cameraIndex*subImageWidth < data.imageRaw().cols);
data.setImageRaw(
cv::Mat(data.imageRaw(),
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone());
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
if(!data.depthOrRightRaw().empty())
{
UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
data.setDepthOrRightRaw(
cv::Mat(data.depthOrRightRaw(),
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone());
}
CameraModel model = data.cameraModels().at(_cameraIndex);
data.setCameraModel(model);
}
else
{
//
UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
}
data.setId(seq);
data.setStamp(stamp);
data.setGroundTruth(groundTruth);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
data.laserScanRaw().empty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1,
data.userDataRaw().empty()?0:1);
if(!this->isKilled())
if(!_odometryIgnored)
{
data.uncompressData();
if(data.cameraModels().size() > 1 &&
_cameraIndex >= 0)
if(pose.isNull())
{
if(_cameraIndex < (int)data.cameraModels().size())
{
// select one camera
int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
UASSERT(!data.imageRaw().empty() &&
data.imageRaw().cols % data.cameraModels().size() == 0 &&
_cameraIndex*subImageWidth < data.imageRaw().cols);
data.setImageRaw(
cv::Mat(data.imageRaw(),
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone());
if(!data.depthOrRightRaw().empty())
{
UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
data.setDepthOrRightRaw(
cv::Mat(data.depthOrRightRaw(),
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone());
}
CameraModel model = data.cameraModels().at(_cameraIndex);
data.setCameraModel(model);
}
else
{
UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
}
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
if(info)
{
info->odomPose = pose;
info->odomCovariance = infMatrix.inv();
}
data.setId(seq);
data.setStamp(stamp);
data.setGroundTruth(groundTruth);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
data.laserScanRaw().empty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1,
data.userDataRaw().empty()?0:1);
odom = OdometryEvent(data, pose, infMatrix.inv());
}
}
}
@@ -420,7 +419,7 @@ OdometryEvent DBReader::getNextData()
{
UERROR("Not initialized...");
}
return odom;
return data;
}
} /* namespace rtabmap */
corelib/src/RtabmapThread.cpp
+1 -1
View File
@@ -333,7 +333,7 @@ void RtabmapThread::handleEvent(UEvent* event)
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeData)
{
this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)
guilib/include/rtabmap/gui/MainWindow.h
-4
View File
@@ -47,9 +47,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class CameraThread;
class DBReader;
class CameraOpenni;
class CameraFreenect;
class OdometryThread;
class CloudViewer;
class LoopClosureViewer;
@@ -254,7 +251,6 @@ private:
State _state;
rtabmap::CameraThread * _camera;
rtabmap::DBReader * _dbReader;
rtabmap::OdometryThread * _odomThread;
//Dialogs
guilib/include/rtabmap/gui/PreferencesDialog.h
+1 -7
View File
@@ -203,13 +203,7 @@ public:
QString getSourceDriverStr() const;
QString getSourceDevice() const;
QString getSourceDatabasePath() const; //Database group
bool getSourceDatabaseOdometryIgnored() const; //Database group
bool getSourceDatabaseGoalDelayIgnored() const; //Database group
bool getSourceDatabaseGoalsIgnored() const; //Database group
int getSourceDatabaseStartPos() const; //Database group
bool getSourceDatabaseStampsUsed() const;//Database group
int getSourceDatabaseCameraIndex() const; //Database group
bool getSourceDatabaseStampsUsed() const;
bool isSourceRGBDColorOnly() const;
int getSourceImageDecimation() const;
bool isSourceStereoDepthGenerated() const;
guilib/src/MainWindow.cpp
+58 -149
View File
@@ -126,7 +126,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui(0),
_state(kIdle),
_camera(0),
_dbReader(0),
_odomThread(0),
_preferencesDialog(0),
_aboutDialog(0),
@@ -671,12 +670,6 @@ void MainWindow::closeEvent(QCloseEvent* event)
delete _camera;
_camera = 0;
}
if(_dbReader)
{
UERROR("DBReader must be already deleted here!");
delete _dbReader;
_dbReader = 0;
}
if(_odomThread)
{
UERROR("OdomThread must be already deleted here!");
@@ -3131,12 +3124,16 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
if(_camera)
{
_camera->setImageRate(_preferencesDialog->getGeneralInputRate());
}
if(_dbReader)
{
_dbReader->setFrameRate( _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
if(dynamic_cast<DBReader*>(_camera->camera()) != 0)
{
_camera->setImageRate( _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
}
else
{
_camera->setImageRate(_preferencesDialog->getGeneralInputRate());
}
}
}//This will update the statistics toolbox
if(flags & PreferencesDialog::kPanelGeneral)
@@ -3798,15 +3795,6 @@ void MainWindow::startDetection()
emit stateChanged(kInitialized);
return;
}
if(_dbReader != 0)
{
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("A database reader is running, stop it first."));
UWARN("_dbReader is not null... it must be stopped first");
emit stateChanged(kInitialized);
return;
}
// Adjust pre-requirements
if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef)
@@ -3820,56 +3808,58 @@ void MainWindow::startDetection()
}
if(_preferencesDialog->getSourceDriver() < PreferencesDialog::kSrcDatabase)
Camera * camera = _preferencesDialog->createCamera();
if(!camera)
{
Camera * camera = _preferencesDialog->createCamera();
if(!camera)
emit stateChanged(kInitialized);
return;
}
_camera = new CameraThread(camera, parameters);
_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
_camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
_camera->setScanFromDepth(
_preferencesDialog->isSourceScanFromDepth(),
_preferencesDialog->getSourceScanFromDepthDecimation(),
_preferencesDialog->getSourceScanFromDepthMaxDepth(),
_preferencesDialog->getSourceScanVoxelSize(),
_preferencesDialog->getSourceScanNormalsK());
//Create odometry thread if rgbd slam
if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
{
// Require calibrated camera
if(!camera->isCalibrated())
{
UWARN("Camera is not calibrated!");
emit stateChanged(kInitialized);
delete _camera;
_camera = 0;
int button = QMessageBox::question(this,
tr("Camera is not calibrated!"),
tr("RTAB-Map cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
QMessageBox::Yes | QMessageBox::No);
if(button == QMessageBox::Yes)
{
QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
}
return;
}
_camera = new CameraThread(camera, parameters);
_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
_camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
_camera->setScanFromDepth(
_preferencesDialog->isSourceScanFromDepth(),
_preferencesDialog->getSourceScanFromDepthDecimation(),
_preferencesDialog->getSourceScanFromDepthMaxDepth(),
_preferencesDialog->getSourceScanVoxelSize(),
_preferencesDialog->getSourceScanNormalsK());
//Create odometry thread if rgbd slam
if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
else
{
// Require calibrated camera
if(!camera->isCalibrated())
if(_odomThread)
{
UWARN("Camera is not calibrated!");
emit stateChanged(kInitialized);
delete _camera;
_camera = 0;
int button = QMessageBox::question(this,
tr("Camera is not calibrated!"),
tr("RTAB-Map cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
QMessageBox::Yes | QMessageBox::No);
if(button == QMessageBox::Yes)
{
QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
}
return;
UERROR("OdomThread must be already deleted here?!");
delete _odomThread;
_odomThread = 0;
}
else
DBReader* dbReader = dynamic_cast<DBReader*>(camera);
if(dbReader == 0 || dbReader->isOdometryIgnored())
{
if(_odomThread)
{
UERROR("OdomThread must be already deleted here?!");
delete _odomThread;
_odomThread = 0;
}
Odometry * odom = Odometry::create(parameters);
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
@@ -3880,65 +3870,10 @@ void MainWindow::startDetection()
}
}
}
else if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
if(_dataRecorder && _camera)
{
_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
_preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceDatabaseOdometryIgnored(),
_preferencesDialog->getSourceDatabaseGoalDelayIgnored(),
_preferencesDialog->getSourceDatabaseGoalsIgnored(),
_preferencesDialog->getSourceDatabaseCameraIndex());
//Create odometry thread if rgdb slam
if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()) &&
_preferencesDialog->getSourceDatabaseOdometryIgnored())
{
if(_odomThread)
{
UERROR("OdomThread must be already deleted here?!");
delete _odomThread;
}
Odometry * odom = Odometry::create(parameters);
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
UEventsManager::addHandler(_odomThread);
_odomThread->start();
}
if(!_dbReader->init(_preferencesDialog->getSourceDatabaseStartPos()))
{
ULOGGER_WARN("init DBReader failed... ");
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Database reader initialization failed..."));
emit stateChanged(kInitialized);
delete _dbReader;
_dbReader = 0;
if(_odomThread)
{
delete _odomThread;
_odomThread = 0;
}
return;
}
if(_odomThread)
{
UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent");
UEventsManager::createPipe(_dbReader, this, "CameraEvent");
}
}
if(_dataRecorder)
{
if(_camera)
{
UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
}
else if(_dbReader)
{
UEventsManager::createPipe(_dbReader, _dataRecorder, "CameraEvent");
}
UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
}
_lastOdomPose.setNull();
@@ -3964,7 +3899,7 @@ void MainWindow::startDetection()
// Could not be in the main thread here! (see handleEvents())
void MainWindow::pauseDetection()
{
if(_camera || _dbReader)
if(_camera)
{
if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
{
@@ -3998,14 +3933,13 @@ void MainWindow::pauseDetection()
void MainWindow::stopDetection()
{
if(!_camera && !_dbReader && !_odomThread)
if(!_camera && !_odomThread)
{
return;
}
if(_state == kDetecting &&
( (_camera && _camera->isRunning()) ||
(_dbReader && _dbReader->isRunning()) ) )
(_camera && _camera->isRunning()) )
{
QMessageBox::StandardButton button = QMessageBox::question(this, tr("Stopping process..."), tr("Are you sure you want to stop the process?"), QMessageBox::Yes|QMessageBox::No, QMessageBox::No);
@@ -4022,11 +3956,6 @@ void MainWindow::stopDetection()
_camera->join(true);
}
if(_dbReader)
{
_dbReader->join(true);
}
if(_odomThread)
{
_ui->actionReset_Odometry->setEnabled(false);
@@ -4039,11 +3968,6 @@ void MainWindow::stopDetection()
delete _camera;
_camera = 0;
}
if(_dbReader)
{
delete _dbReader;
_dbReader = 0;
}
if(_odomThread)
{
delete _odomThread;
@@ -5923,11 +5847,6 @@ void MainWindow::changeState(MainWindow::State newState)
{
_camera->start();
}
if(_dbReader)
{
_dbReader->start();
}
break;
case kPaused:
@@ -5959,11 +5878,6 @@ void MainWindow::changeState(MainWindow::State newState)
{
_camera->start();
}
if(_dbReader)
{
_dbReader->start();
}
}
else if(_state == kDetecting)
{
@@ -5993,11 +5907,6 @@ void MainWindow::changeState(MainWindow::State newState)
{
_camera->join(true);
}
if(_dbReader)
{
_dbReader->join(true);
}
}
break;
case kMonitoring:
guilib/src/PreferencesDialog.cpp
+50 -133
View File
@@ -3520,7 +3520,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
_ui->groupBox_sourceRGB->setVisible(_ui->comboBox_sourceType->currentIndex() == 2);
_ui->groupBox_sourceDatabase->setVisible(_ui->comboBox_sourceType->currentIndex() == 3);
_ui->groupBox_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1);
_ui->groupBox_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3);
_ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 &&
(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD ||
@@ -3549,7 +3549,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB));
_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3);
//_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3);
_ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
}
@@ -3891,30 +3891,6 @@ Transform PreferencesDialog::getLaserLocalTransform() const
return t;
}
QString PreferencesDialog::getSourceDatabasePath() const
{
return _ui->source_database_lineEdit_path->text();
}
bool PreferencesDialog::getSourceDatabaseOdometryIgnored() const
{
return _ui->source_checkBox_ignoreOdometry->isChecked();
}
bool PreferencesDialog::getSourceDatabaseGoalDelayIgnored() const
{
return _ui->source_checkBox_ignoreGoalDelay->isChecked();
}
bool PreferencesDialog::getSourceDatabaseGoalsIgnored() const
{
return _ui->source_checkBox_ignoreGoals->isChecked();
}
int PreferencesDialog::getSourceDatabaseStartPos() const
{
return _ui->source_spinBox_databaseStartPos->value();
}
int PreferencesDialog::getSourceDatabaseCameraIndex() const
{
return _ui->source_spinBox_database_cameraIndex->value();
}
bool PreferencesDialog::getSourceDatabaseStampsUsed() const
{
return _ui->source_checkBox_useDbStamps->isChecked();
@@ -4193,8 +4169,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
}
else if(driver == kSrcDatabase)
{
UERROR("Call directly DBReader for kSrcDatabase.");
return 0;
camera = new DBReader(_ui->source_database_lineEdit_path->text().toStdString(),
_ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
_ui->source_checkBox_ignoreOdometry->isChecked(),
_ui->source_checkBox_ignoreGoalDelay->isChecked(),
_ui->source_checkBox_ignoreGoals->isChecked(),
_ui->source_spinBox_databaseStartPos->value(),
_ui->source_spinBox_database_cameraIndex->value());
}
else
{
@@ -4376,30 +4357,11 @@ void PreferencesDialog::setSLAMMode(bool enabled)
void PreferencesDialog::testOdometry()
{
DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
_ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
true,
true,
true,
_ui->source_spinBox_database_cameraIndex->value());
Camera * camera = 0;
if(this->getSourceType() == kSrcDatabase)
Camera * camera = this->createCamera();
if(!camera)
{
if(!dbReader.init())
{
QMessageBox::warning(this, tr("Camera viewer"), tr("Failed to initialize the database reader!"));
return;
}
return;
}
else
{
camera = this->createCamera();
if(!camera)
{
return;
}
}
ParametersMap parameters = this->getAllParameters();
Odometry * odometry = Odometry::create(parameters);
@@ -4420,9 +4382,42 @@ void PreferencesDialog::testOdometry()
odomViewer->resize(1280, 480+QPushButton().minimumHeight());
odomViewer->registerToEventsManager();
CameraThread cameraThread(camera, this->getAllParameters()); // take ownership of camera
cameraThread.setMirroringEnabled(isSourceMirroring());
cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
cameraThread.setScanFromDepth(
_ui->groupBox_scanFromDepth->isChecked(),
_ui->spinBox_cameraScanFromDepth_decimation->value(),
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value());
UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
odomThread.start();
cameraThread.start();
odomViewer->exec();
delete odomViewer;
cameraThread.join(true);
odomThread.join(true);
}
void PreferencesDialog::testCamera()
{
CameraViewer * window = new CameraViewer(this, this->getAllParameters());
window->setWindowTitle(tr("Camera viewer"));
window->resize(1280, 480+QPushButton().minimumHeight());
window->registerToEventsManager();
Camera * camera = this->createCamera();
if(camera)
{
CameraThread cameraThread(camera, this->getAllParameters()); // take ownership of camera
CameraThread cameraThread(camera, this->getAllParameters());
cameraThread.setMirroringEnabled(isSourceMirroring());
cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
@@ -4433,97 +4428,19 @@ void PreferencesDialog::testOdometry()
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value());
UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
odomThread.start();
cameraThread.start();
odomViewer->exec();
delete odomViewer;
window->exec();
delete window;
cameraThread.join(true);
odomThread.join(true);
}
else
{
UEventsManager::createPipe(&dbReader, &odomThread, "CameraEvent");
UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
odomThread.start();
dbReader.start();
odomViewer->exec();
delete odomViewer;
dbReader.join(true);
odomThread.join(true);
delete window;
}
}
void PreferencesDialog::testCamera()
{
CameraViewer * window = new CameraViewer(this, this->getAllParameters());
window->setWindowTitle(tr("Camera viewer"));
window->resize(1280, 480+QPushButton().minimumHeight());
window->registerToEventsManager();
if(this->getSourceType() == kSrcDatabase)
{
DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
_ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
true,
true,
true,
_ui->source_spinBox_database_cameraIndex->value());
if(!dbReader.init(_ui->source_images_spinBox_startPos->value()))
{
QMessageBox::warning(this, tr("Camera viewer"), tr("Failed to initialize the database reader!"));
delete window;
}
else
{
UEventsManager::createPipe(&dbReader, window, "CameraEvent");
dbReader.start();
window->exec();
delete window;
dbReader.join(true);
}
}
else
{
Camera * camera = this->createCamera();
if(camera)
{
CameraThread cameraThread(camera, this->getAllParameters());
cameraThread.setMirroringEnabled(isSourceMirroring());
cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
cameraThread.setScanFromDepth(
_ui->groupBox_scanFromDepth->isChecked(),
_ui->spinBox_cameraScanFromDepth_decimation->value(),
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value());
UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
cameraThread.start();
window->exec();
delete window;
cameraThread.join(true);
}
else
{
delete window;
}
}
}
void PreferencesDialog::calibrate()
{
if(this->getSourceType() == kSrcDatabase)
tools/Camera/main.cpp
+3 -18
View File
@@ -132,7 +132,6 @@ int main(int argc, char * argv[])
}
rtabmap::Camera * camera = 0;
rtabmap::DBReader * dbReader = 0;
if(!path.empty())
{
@@ -140,7 +139,7 @@ int main(int argc, char * argv[])
{
if(UFile::getExtension(path).compare("db") == 0)
{
dbReader = new rtabmap::DBReader(path, rate);
camera = new rtabmap::DBReader(path, rate);
}
else
{
@@ -176,18 +175,8 @@ int main(int argc, char * argv[])
}
}
if(dbReader)
{
if(!dbReader->init())
{
delete dbReader;
UERROR("Cannot initialize the camera.");
return -1;
}
}
cv::Mat rgb;
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
rgb = camera->takeImage().imageRaw();
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
while(!rgb.empty())
{
@@ -197,16 +186,12 @@ int main(int argc, char * argv[])
if(c == 27)
break; // if ESC, break and quit
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
rgb = camera->takeImage().imageRaw();
}
cv::destroyWindow("Video");
if(camera)
{
delete camera;
}
if(dbReader)
{
delete dbReader;
}
return 0;
}
tools/OdometryViewer/main.cpp
+96 -114
View File
@@ -223,131 +223,113 @@ int main (int argc, char * argv[])
odomViewer.setWindowTitle("Odometry view");
odomViewer.resize(1280, 480+QPushButton().minimumHeight());
rtabmap::Camera * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(inputDatabase.size())
{
rtabmap::DBReader camera(inputDatabase, rate, true);
if(camera.init())
camera = new rtabmap::DBReader(inputDatabase, rate, true);
}
else if(driver == 0)
{
camera = new rtabmap::CameraOpenni("", rate, t);
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2("", rate, t);
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect(0, rate, t);
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false, rate, t);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true, rate, t);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with dc1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394(rate, t);
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
}
else
{
UFATAL("Camera driver (%d) not found!", driver);
}
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
if(camera->init())
{
if(camera->isCalibrated())
{
rtabmap::CameraThread cameraThread(camera, parameters);
cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK);
odomThread.start();
cameraThread.start();
if(sec > 0)
{
uSleep(sec*1000);
}
odomViewer.exec();
camera.start();
app.exec();
camera.join(true);
cameraThread.join(true);
odomThread.join(true);
}
else
{
printf("The camera is not calibrated! You should calibrate the camera first.\n");
delete camera;
}
}
else
{
rtabmap::Camera * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 0)
{
camera = new rtabmap::CameraOpenni("", rate, t);
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2("", rate, t);
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect(0, rate, t);
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false, rate, t);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true, rate, t);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with dc1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394(rate, t);
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
}
else
{
UFATAL("Camera driver (%d) not found!", driver);
}
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
if(camera->init())
{
if(camera->isCalibrated())
{
rtabmap::CameraThread cameraThread(camera, parameters);
cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK);
odomThread.start();
cameraThread.start();
odomViewer.exec();
cameraThread.join(true);
odomThread.join(true);
}
else
{
printf("The camera is not calibrated! You should calibrate the camera first.\n");
delete camera;
}
}
else
{
printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
delete camera;
}
printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
delete camera;
}
return 0;