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

corelib/include/rtabmap/core/RtabmapThread.h

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

corelib/src/DBReader.cpp

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

corelib/src/Odometry.cpp

@@ -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())
 			{

corelib/src/RtabmapThread.cpp

@@ -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;
 }
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -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;

guilib/src/MainWindow.cpp

@@ -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.loadFromData(iter.value(), _autoScreenCapturePNG?"PNG":"JPEG");
-			figure.save(targetDir + iter.key(), "PNG");
+			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);
 	}
 }