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

Merge branch 'master' of https://github.com/introlab/rtabmap into devel

Browse Source
This commit is contained in:
matlabbe
2017-12-11 17:10:08 -05:00
parent 775b80eff5 d776092b35
commit f973fc3743
6 changed files with 48 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -113,6 +113,7 @@ private:
std::queue<ParametersMap> _stateParam;
std::list<OdometryEvent> _dataBuffer;
std::list<double> _newMapEvents;
UMutex _dataMutex;
USemaphore _dataAdded;
unsigned int _dataBufferMaxSize;

4
corelib/src/DBReader.cpp
View File

@@ -401,6 +401,10 @@ SensorData DBReader::getNextData(CameraInfo * info)
_previousStamp = stamp;
_previousMapID = mapId;
}
else
{
stamp = 0;
}
data.uncompressData();
if(data.cameraModels().size() > 1 &&

2
corelib/src/Odometry.cpp
View File

@@ -235,7 +235,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UTimer alignTimer;
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 10, 0, indices.get());
bool success = false;
if(indices->size())
{

22
corelib/src/RtabmapThread.cpp
View File

@@ -85,6 +85,7 @@ void RtabmapThread::clearBufferedData()
_dataMutex.lock();
{
_dataBuffer.clear();
_newMapEvents.clear();
lastPose_.setIdentity();
covariance_ = cv::Mat();
_previousStamp = 0;
@@ -572,13 +573,13 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
{
if(odomEvent.pose().isIdentity())
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
UWARN("Odometry is reset (identity pose detected). Increment map id! (stamp=%fs)", odomEvent.data().stamp());
}
else
{
UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0));
UWARN("Odometry is reset (high variance (%f >=9999 detected, stamp=%fs). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0), odomEvent.data().stamp());
}
pushNewState(kStateTriggeringMap);
_newMapEvents.push_back(odomEvent.data().stamp());
covariance_ = cv::Mat();
}
@@ -654,7 +655,7 @@ bool RtabmapThread::getData(OdometryEvent & data)
ULOGGER_INFO("wake-up");
bool dataFilled = false;
bool triggerNewMap = false;
_dataMutex.lock();
{
if(_state.empty() && !_dataBuffer.empty())
@@ -672,10 +673,23 @@ bool RtabmapThread::getData(OdometryEvent & data)
}
_userDataMutex.unlock();
while(_newMapEvents.size() && _newMapEvents.front() <= data.data().stamp())
{
UWARN("Triggering new map %f<=%f...", _newMapEvents.front() , data.data().stamp());
triggerNewMap = true;
_newMapEvents.pop_front();
}
dataFilled = true;
}
}
_dataMutex.unlock();
if(triggerNewMap)
{
_rtabmap->triggerNewMap();
}
return dataFilled;
}

3
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -263,7 +263,7 @@ private:
void saveFigures();
void loadFigures();
void exportPoses(int format);
QString captureScreen(bool cacheInRAM = false);
QString captureScreen(bool cacheInRAM = false, bool png = true);
protected:
Ui_mainWindow * ui() { return _ui; }
@@ -366,6 +366,7 @@ private:
QMap<int, QString> _exportPosesFileName;
bool _autoScreenCaptureOdomSync;
bool _autoScreenCaptureRAM;
bool _autoScreenCapturePNG;
QMap<QString, QByteArray> _autoScreenCaptureCachedImages;
QVector<int> _refIds;

30
guilib/src/MainWindow.cpp
View File

@@ -168,6 +168,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_exportPosesFrame(0),
_autoScreenCaptureOdomSync(false),
_autoScreenCaptureRAM(false),
_autoScreenCapturePNG(false),
_firstCall(true),
_progressCanceled(false)
{
@@ -1404,7 +1405,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
{
this->captureScreen(_autoScreenCaptureRAM);
this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
}
//Process info
@@ -1975,7 +1976,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime, _preferencesDialog->isCacheSavedInFigures());
if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
{
this->captureScreen(_autoScreenCaptureRAM);
this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
}
if(!_preferencesDialog->isImagesKept())
@@ -4110,9 +4111,9 @@ void MainWindow::changeMappingMode()
emit mappingModeChanged(_ui->actionSLAM_mode->isChecked());
}
QString MainWindow::captureScreen(bool cacheInRAM)
QString MainWindow::captureScreen(bool cacheInRAM, bool png)
{
QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".png");
QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (png?".png":".jpg"));
_ui->statusbar->clearMessage();
QPixmap figure = QPixmap::grabWidget(this);
@@ -4124,7 +4125,7 @@ QString MainWindow::captureScreen(bool cacheInRAM)
QByteArray bytes;
QBuffer buffer(&bytes);
buffer.open(QIODevice::WriteOnly);
figure.save(&buffer, "PNG");
figure.save(&buffer, png?"PNG":"JPEG");
_autoScreenCaptureCachedImages.insert(name, bytes);
}
else
@@ -6152,6 +6153,16 @@ void MainWindow::selectScreenCaptureFormat(bool checked)
{
_ui->actionAuto_screen_capture->setChecked(false);
}
r = QMessageBox::question(this, tr("Save in JPEG?"), tr("Save in JPEG format? Otherwise they are saved in PNG."), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
if(r == QMessageBox::No || r == QMessageBox::Yes)
{
_autoScreenCapturePNG = r == QMessageBox::No;
}
else
{
_ui->actionAuto_screen_capture->setChecked(false);
}
}
}
else
@@ -6175,20 +6186,23 @@ void MainWindow::selectScreenCaptureFormat(bool checked)
}
targetDir += QDir::separator();
_progressDialog->setCancelButtonVisible(true);
_progressDialog->resetProgress();
_progressDialog->show();
_progressDialog->setMaximumSteps(_autoScreenCaptureCachedImages.size());
int i=0;
for(QMap<QString, QByteArray>::iterator iter=_autoScreenCaptureCachedImages.begin(); iter!=_autoScreenCaptureCachedImages.end(); ++iter)
for(QMap<QString, QByteArray>::iterator iter=_autoScreenCaptureCachedImages.begin(); iter!=_autoScreenCaptureCachedImages.end() && !_progressDialog->isCanceled(); ++iter)
{
QPixmap figure;
figure.loadFromData(iter.value(), "PNG");
figure.save(targetDir + iter.key(), "PNG");
figure.loadFromData(iter.value(), _autoScreenCapturePNG?"PNG":"JPEG");
figure.save(targetDir + iter.key(), _autoScreenCapturePNG?"PNG":"JPEG");
_progressDialog->appendText(tr("Saved image \"%1\" (%2/%3).").arg(targetDir + iter.key()).arg(++i).arg(_autoScreenCaptureCachedImages.size()));
_progressDialog->incrementStep();
QApplication::processEvents();
}
_autoScreenCaptureCachedImages.clear();
_progressDialog->setValue(_progressDialog->maximumSteps());
_progressDialog->setCancelButtonVisible(false);
}
}